diff --git a/src/GPS/CMakeLists.txt b/src/GPS/CMakeLists.txt
deleted file mode 100644
index d969215..0000000
--- a/src/GPS/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-add_library(gps
-	Drivers/src/ashtech.cpp
-	Drivers/src/gps_helper.cpp
-	Drivers/src/mtk.cpp
-	Drivers/src/rtcm.cpp
-	Drivers/src/sbf.cpp
-	Drivers/src/ubx.cpp
-	GPSManager.cc
-	GPSProvider.cc
-	RTCM/RTCMMavlink.cc
-)
-
-target_link_libraries(gps
-	Qt6::Core
-	Qt6::Location
-	Qt6::SerialPort
-	Qt6::Svg
-	Qt6::TextToSpeech
-
-	qgc
-)
-
-target_include_directories(gps INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
-
diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h
deleted file mode 100644
index dffa72e..0000000
--- a/src/GPS/GPSManager.h
+++ /dev/null
@@ -1,42 +0,0 @@
-
-
-#pragma once
-
-#include "GPSProvider.h"
-#include "RTCM/RTCMMavlink.h"
-#include <QGCToolbox.h>
-
-#include <QString>
-#include <QObject>
-
-/**
- ** class GPSManager
- * handles a GPS provider and RTK
- */
-class GPSManager : public QGCTool
-{
-    Q_OBJECT
-public:
-    GPSManager(QGCApplication* app, QGCToolbox* toolbox);
-    ~GPSManager();
-
-    void connectGPS     (const QString& device, const QString& gps_type);
-    void disconnectGPS  (void);
-    bool connected      (void) const { return _gpsProvider && _gpsProvider->isRunning(); }
-
-signals:
-    void onConnect();
-    void onDisconnect();
-    void surveyInStatus(float duration, float accuracyMM,  double latitude, double longitude, float altitude, bool valid, bool active);
-    void satelliteUpdate(int numSats);
-
-private slots:
-    void GPSPositionUpdate(GPSPositionMessage msg);
-    void GPSSatelliteUpdate(GPSSatelliteMessage msg);
-
-private:
-    GPSProvider* _gpsProvider = nullptr;
-    RTCMMavlink* _rtcmMavlink = nullptr;
-
-    std::atomic_bool _requestGpsStop; ///< signals the thread to quit
-};
diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h
deleted file mode 100644
index 3f5ba29..0000000
--- a/src/GPS/GPSPositionMessage.h
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
-#pragma once
-
-#include "vehicle_gps_position.h"
-#include "satellite_info.h"
-#include <QMetaType>
-
-/**
- ** struct GPSPositionMessage
- * wrapper that can be used for Qt signal/slots
- */
-struct GPSPositionMessage
-{
-    sensor_gps_s position_data;
-};
-
-Q_DECLARE_METATYPE(GPSPositionMessage);
-
-
-struct GPSSatelliteMessage
-{
-    satellite_info_s satellite_data;
-};
-Q_DECLARE_METATYPE(GPSSatelliteMessage);
diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h
deleted file mode 100644
index a901f8b..0000000
--- a/src/GPS/GPSProvider.h
+++ /dev/null
@@ -1,84 +0,0 @@
-
-#pragma once
-
-#include <QString>
-#include <QThread>
-#include <QByteArray>
-#include <QSerialPort>
-
-#include <atomic>
-
-#include "GPSPositionMessage.h"
-#include "Drivers/src/gps_helper.h"
-
-
-/**
- ** class GPSProvider
- * opens a GPS device and handles the protocol
- */
-class GPSProvider : public QThread
-{
-    Q_OBJECT
-public:
-
-    enum class GPSType {
-        u_blox,
-        trimble,
-        septentrio
-    };
-
-    GPSProvider(const QString& device,
-                GPSType type,
-                bool    enableSatInfo,
-                double  surveyInAccMeters,
-                int     surveryInDurationSecs,
-                bool    useFixedBaseLocation,
-                double  fixedBaseLatitude,
-                double  fixedBaseLongitude,
-                float   fixedBaseAltitudeMeters,
-                float   fixedBaseAccuracyMeters,
-                const std::atomic_bool& requestStop);
-    ~GPSProvider();
-
-    /**
-     * this is called by the callback method
-     */
-    void gotRTCMData(uint8_t *data, size_t len);
-
-signals:
-    void positionUpdate(GPSPositionMessage message);
-    void satelliteInfoUpdate(GPSSatelliteMessage message);
-    void RTCMDataUpdate(QByteArray message);
-    void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active);
-
-protected:
-    void run();
-
-private:
-    void publishGPSPosition();
-    void publishGPSSatellite();
-
-	/**
-	 * callback from the driver for the platform specific stuff
-	 */
-	static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user);
-
-	int callback(GPSCallbackType type, void *data1, int data2);
-
-    QString _device;
-    GPSType _type;
-    const std::atomic_bool& _requestStop;
-    double  _surveyInAccMeters;
-    int     _surveryInDurationSecs;
-    bool    _useFixedBaseLoction;
-    double  _fixedBaseLatitude;
-    double  _fixedBaseLongitude;
-    float   _fixedBaseAltitudeMeters;
-    float   _fixedBaseAccuracyMeters;
-    GPSHelper::GPSConfig _gpsConfig{};
-
-	struct sensor_gps_s        _reportGpsPos;
-	struct satellite_info_s    *_pReportSatInfo = nullptr;
-
-	QSerialPort *_serial = nullptr;
-};
diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc
deleted file mode 100644
index 19dedd7..0000000
--- a/src/GPS/RTCM/RTCMMavlink.cc
+++ /dev/null
@@ -1,76 +0,0 @@
-
-
-
-#include "RTCMMavlink.h"
-
-#include "MultiVehicleManager.h"
-#include "Vehicle.h"
-
-#include <cstdio>
-
-RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
-    : _toolbox(toolbox)
-{
-    _bandwidthTimer.start();
-}
-
-void RTCMMavlink::RTCMDataUpdate(QByteArray message)
-{
-    /* statistics */
-    _bandwidthByteCounter += message.size();
-    qint64 elapsed = _bandwidthTimer.elapsed();
-    if (elapsed > 1000) {
-        printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f);
-        _bandwidthTimer.restart();
-        _bandwidthByteCounter = 0;
-    }
-
-    const qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
-    mavlink_gps_rtcm_data_t mavlinkRtcmData;
-    memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
-
-    if (message.size() < maxMessageLength) {
-        mavlinkRtcmData.len = message.size();
-        mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
-        memcpy(&mavlinkRtcmData.data, message.data(), message.size());
-        sendMessageToVehicle(mavlinkRtcmData);
-    } else {
-        // We need to fragment
-
-        uint8_t fragmentId = 0;         // Fragment id indicates the fragment within a set
-        int start = 0;
-        while (start < message.size()) {
-            int length = std::min(message.size() - start, maxMessageLength);
-            mavlinkRtcmData.flags = 1;                      // LSB set indicates message is fragmented
-            mavlinkRtcmData.flags |= fragmentId++ << 1;     // Next 2 bits are fragment id
-            mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3;     // Next 5 bits are sequence id
-            mavlinkRtcmData.len = length;
-            memcpy(&mavlinkRtcmData.data, message.data() + start, length);
-            sendMessageToVehicle(mavlinkRtcmData);
-            start += length;
-        }
-    }
-    ++_sequenceId;
-}
-
-void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
-{
-    QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
-    MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
-    for (int i = 0; i < vehicles.count(); i++) {
-        Vehicle*                vehicle     = qobject_cast<Vehicle*>(vehicles[i]);
-        WeakLinkInterfacePtr    weakLink    = vehicle->vehicleLinkManager()->primaryLink();
-
-        if (!weakLink.expired()) {
-            mavlink_message_t       message;
-            SharedLinkInterfacePtr  sharedLink = weakLink.lock();
-
-            mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(),
-                                                  mavlinkProtocol->getComponentId(),
-                                                  sharedLink->mavlinkChannel(),
-                                                  &message,
-                                                  &msg);
-            vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
-        }
-    }
-}
diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h
deleted file mode 100644
index a5d4bbc..0000000
--- a/src/GPS/RTCM/RTCMMavlink.h
+++ /dev/null
@@ -1,33 +0,0 @@
-
-
-
-#pragma once
-
-#include <QObject>
-#include <QElapsedTimer>
-
-#include "QGCToolbox.h"
-#include "MAVLinkProtocol.h"
-
-/**
- ** class RTCMMavlink
- * Receives RTCM updates and sends them via MAVLINK to the device
- */
-class RTCMMavlink : public QObject
-{
-    Q_OBJECT
-public:
-    RTCMMavlink(QGCToolbox& toolbox);
-    //TODO: API to select device(s)?
-
-public slots:
-    void RTCMDataUpdate(QByteArray message);
-
-private:
-    void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg);
-
-    QGCToolbox& _toolbox;
-    QElapsedTimer _bandwidthTimer;
-    int _bandwidthByteCounter = 0;
-    uint8_t _sequenceId = 0;
-};
diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h
deleted file mode 100644
index b4377d3..0000000
--- a/src/GPS/definitions.h
+++ /dev/null
@@ -1,63 +0,0 @@
-
-
-/**
- * @file definitions.h
- * common platform-specific definitions & abstractions for gps
- * @author Beat K眉ng <beat-kueng@gmx.net>
- */
-
-#pragma once
-
-#include <QtGlobal>
-
-#define GPS_READ_BUFFER_SIZE 1024
-
-#define GPS_INFO(...) qInfo(__VA_ARGS__)
-#define GPS_WARN(...) qWarning(__VA_ARGS__)
-#define GPS_ERR(...) qCritical(__VA_ARGS__)
-
-#include "vehicle_gps_position.h"
-#include "satellite_info.h"
-
-#define M_DEG_TO_RAD 		(M_PI / 180.0)
-#define M_RAD_TO_DEG 		(180.0 / M_PI)
-#define M_DEG_TO_RAD_F 		0.01745329251994f
-#define M_RAD_TO_DEG_F 		57.2957795130823f
-
-#include <QThread>
-
-class Sleeper : public QThread
-{
-public:
-    static void usleep(unsigned long usecs) { QThread::usleep(usecs); }
-};
-
-static inline void gps_usleep(unsigned long usecs) {
-    Sleeper::usleep(usecs);
-}
-
-typedef uint64_t gps_abstime;
-
-#include <QDateTime>
-/**
- * Get the current time in us. Function signature:
- * uint64_t hrt_absolute_time()
- */
-static inline gps_abstime gps_absolute_time() {
-    //FIXME: is there something with microsecond accuracy?
-    return QDateTime::currentMSecsSinceEpoch() * 1000;
-}
-
-//timespec is UNIX-specific
-#ifdef _WIN32
-#if _MSC_VER < 1900
-struct timespec
-{
-    time_t tv_sec;
-    long tv_nsec;
-};
-#else
-#include <time.h>
-#endif
-#endif
-
diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h
deleted file mode 100644
index e2d1450..0000000
--- a/src/GPS/satellite_info.h
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-#pragma once
-#include <stdint.h>
-
-/*
- * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg
- * and was manually copied here.
- */
-
-struct satellite_info_s {
-	uint64_t timestamp;
-	uint8_t count;
-	uint8_t svid[20];
-	uint8_t used[20];
-	uint8_t elevation[20];
-	uint8_t azimuth[20];
-	uint8_t snr[20];
-	uint8_t prn[20];
-#ifdef __cplusplus
-	static const uint8_t SAT_INFO_MAX_SATELLITES = 20;
-
-#endif
-};
diff --git a/src/GPS/vehicle_gps_position.h b/src/GPS/vehicle_gps_position.h
deleted file mode 100644
index 3e2b589..0000000
--- a/src/GPS/vehicle_gps_position.h
+++ /dev/null
@@ -1,39 +0,0 @@
-
-
-
-#pragma once
-#include <stdint.h>
-
-/*
- * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg
- * and was manually copied here.
- */
-
-struct sensor_gps_s {
-	uint64_t timestamp;
-	uint64_t time_utc_usec;
-	int32_t lat;
-	int32_t lon;
-	int32_t alt;
-	int32_t alt_ellipsoid;
-	uint16_t automatic_gain_control;
-	uint8_t jamming_state;
-	float s_variance_m_s;
-	float c_variance_rad;
-	float eph;
-	float epv;
-	float hdop;
-	float vdop;
-	int32_t noise_per_ms;
-	int32_t jamming_indicator;
-	float vel_m_s;
-	float vel_n_m_s;
-	float vel_e_m_s;
-	float vel_d_m_s;
-	float cog_rad;
-	int32_t timestamp_time_relative;
-	float heading;
-	uint8_t fix_type;
-	bool vel_ned_valid;
-	uint8_t satellites_used;
-};
diff --git a/src/GPS/GPSManager.cc b/src/GPSManager.cpp
similarity index 100%
rename from src/GPS/GPSManager.cc
rename to src/GPSManager.cpp
diff --git a/src/GPS/GPSProvider.cc b/src/GPSProvider.cpp
similarity index 100%
rename from src/GPS/GPSProvider.cc
rename to src/GPSProvider.cpp
diff --git a/src/JsonHelper.cpp b/src/JsonHelper.cpp
new file mode 100644
index 0000000..e5aa783
--- /dev/null
+++ b/src/JsonHelper.cpp
@@ -0,0 +1,520 @@
+/****************************************************************************
+ *此代码是QGC开发的重要部分 
+ ****************************************************************************/
+
+#include "JsonHelper.h"
+#include "QGCQGeoCoordinate.h"
+#include "QmlObjectListModel.h"
+#include "MissionCommandList.h"
+#include "FactMetaData.h"
+#include "QGCApplication.h"
+
+#include <QJsonArray>
+#include <QJsonParseError>
+#include <QObject>
+#include <QRegularExpression>
+#include <QRegularExpressionMatch>
+#include <QFile>
+#include <QTranslator>
+
+const char* JsonHelper::jsonVersionKey                      = "version";
+const char* JsonHelper::jsonGroundStationKey                = "groundStation";
+const char* JsonHelper::jsonGroundStationValue              = "QGroundControl";
+const char* JsonHelper::jsonFileTypeKey                     = "fileType";
+const char* JsonHelper::_translateKeysKey                   = "translateKeys";
+const char* JsonHelper::_arrayIDKeysKey                     = "_arrayIDKeys";
+
+bool JsonHelper::validateRequiredKeys(const QJsonObject& jsonObject, const QStringList& keys, QString& errorString)
+{
+    QString missingKeys;
+
+    foreach(const QString& key, keys) {
+        if (!jsonObject.contains(key)) {
+            if (!missingKeys.isEmpty()) {
+                missingKeys += QStringLiteral(", ");
+            }
+            missingKeys += key;
+        }
+    }
+
+    if (missingKeys.length() != 0) {
+        errorString = QObject::tr("The following required keys are missing: %1").arg(missingKeys);
+        return false;
+    }
+
+    return true;
+}
+
+bool JsonHelper::_loadGeoCoordinate(const QJsonValue&   jsonValue,
+                                    bool                altitudeRequired,
+                                    QGeoCoordinate&     coordinate,
+                                    QString&            errorString,
+                                    bool                geoJsonFormat)
+{
+    if (!jsonValue.isArray()) {
+        errorString = QObject::tr("value for coordinate is not array");
+        return false;
+    }
+
+    QJsonArray coordinateArray = jsonValue.toArray();
+    int requiredCount = altitudeRequired ? 3 : 2;
+    if (coordinateArray.count() != requiredCount) {
+        errorString = QObject::tr("Coordinate array must contain %1 values").arg(requiredCount);
+        return false;
+    }
+
+    for (const auto& jsonValue: coordinateArray) {
+        if (jsonValue.type() != QJsonValue::Double && jsonValue.type() != QJsonValue::Null) {
+            errorString = QObject::tr("Coordinate array may only contain double values, found: %1").arg(jsonValue.type());
+            return false;
+        }
+    }
+
+    if (geoJsonFormat) {
+        coordinate = QGeoCoordinate(coordinateArray[1].toDouble(), coordinateArray[0].toDouble());
+    } else {
+        coordinate = QGeoCoordinate(possibleNaNJsonValue(coordinateArray[0]), possibleNaNJsonValue(coordinateArray[1]));
+    }
+    if (altitudeRequired) {
+        coordinate.setAltitude(possibleNaNJsonValue(coordinateArray[2]));
+    }
+
+    return true;
+}
+
+void JsonHelper::_saveGeoCoordinate(const QGeoCoordinate&   coordinate,
+                                    bool                    writeAltitude,
+                                    QJsonValue&             jsonValue,
+                                    bool                    geoJsonFormat)
+{
+    QJsonArray coordinateArray;
+
+    if (geoJsonFormat) {
+        coordinateArray << coordinate.longitude() << coordinate.latitude();
+    } else {
+        coordinateArray << coordinate.latitude() << coordinate.longitude();
+    }
+    if (writeAltitude) {
+        coordinateArray << coordinate.altitude();
+    }
+
+    jsonValue = QJsonValue(coordinateArray);
+}
+
+bool JsonHelper::loadGeoCoordinate(const QJsonValue&    jsonValue,
+                                   bool                 altitudeRequired,
+                                   QGeoCoordinate&      coordinate,
+                                   QString&             errorString,
+                                   bool                 geoJsonFormat)
+{
+    return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, geoJsonFormat);
+}
+
+void JsonHelper::saveGeoCoordinate(const QGeoCoordinate&    coordinate,
+                                   bool                     writeAltitude,
+                                   QJsonValue&              jsonValue)
+{
+    _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, false /* geoJsonFormat */);
+}
+
+bool JsonHelper::loadGeoJsonCoordinate(const QJsonValue& jsonValue,
+                                       bool              altitudeRequired,
+                                       QGeoCoordinate&   coordinate,
+                                       QString&          errorString)
+{
+    return _loadGeoCoordinate(jsonValue, altitudeRequired, coordinate, errorString, true /* geoJsonFormat */);
+}
+
+void JsonHelper::saveGeoJsonCoordinate(const QGeoCoordinate& coordinate,
+                                       bool                  writeAltitude,
+                                       QJsonValue&           jsonValue)
+{
+    _saveGeoCoordinate(coordinate, writeAltitude, jsonValue, true /* geoJsonFormat */);
+}
+
+bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringList& keys, const QList<QJsonValue::Type>& types, QString& errorString)
+{
+    for (int i=0; i<types.count(); i++) {
+        QString valueKey = keys[i];
+        if (jsonObject.contains(valueKey)) {
+            const QJsonValue& jsonValue = jsonObject[valueKey];
+            if (jsonValue.type() == QJsonValue::Null &&  types[i] == QJsonValue::Double) {
+                // Null type signals a NaN on a double value
+                continue;
+            }
+            if (jsonValue.type() != types[i]) {
+                errorString  = QObject::tr("Incorrect value type - key:type:expected %1:%2:%3").arg(valueKey).arg(_jsonValueTypeToString(jsonValue.type())).arg(_jsonValueTypeToString(types[i]));
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
+bool JsonHelper::isJsonFile(const QByteArray& bytes, QJsonDocument& jsonDoc, QString& errorString)
+{
+    QJsonParseError parseError;
+
+    jsonDoc = QJsonDocument::fromJson(bytes, &parseError);
+
+    if (parseError.error == QJsonParseError::NoError) {
+        return true;
+    } else {
+        int startPos = qMax(0, parseError.offset - 100);
+        int length = qMin(bytes.length() - startPos, 200);
+        qDebug() << QStringLiteral("Json read error '%1'").arg(bytes.mid(startPos, length).constData());
+        errorString = parseError.errorString();
+        return false;
+    }
+}
+
+bool JsonHelper::isJsonFile(const QString& fileName, QJsonDocument& jsonDoc, QString& errorString)
+{
+    QFile jsonFile(fileName);
+    if (!jsonFile.open(QFile::ReadOnly)) {
+        errorString = tr("File open failed: file:error %1 %2").arg(jsonFile.fileName()).arg(jsonFile.errorString());
+        return false;
+    }
+    QByteArray jsonBytes = jsonFile.readAll();
+    jsonFile.close();
+
+    return isJsonFile(jsonBytes, jsonDoc, errorString);
+}
+
+bool JsonHelper::validateInternalQGCJsonFile(const QJsonObject& jsonObject,
+                                             const QString&     expectedFileType,
+                                             int                minSupportedVersion,
+                                             int                maxSupportedVersion,
+                                             int&               version,
+                                             QString&           errorString)
+{
+    // Validate required keys
+    QList<JsonHelper::KeyValidateInfo> requiredKeys = {
+        { jsonFileTypeKey,       QJsonValue::String, true },
+        { jsonVersionKey,        QJsonValue::Double, true },
+    };
+    if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) {
+        return false;
+    }
+
+    // Make sure file type is correct
+    QString fileTypeValue = jsonObject[jsonFileTypeKey].toString();
+    if (fileTypeValue != expectedFileType) {
+        errorString = QObject::tr("Incorrect file type key expected:%1 actual:%2").arg(expectedFileType).arg(fileTypeValue);
+        return false;
+    }
+
+    // Version check
+    version = jsonObject[jsonVersionKey].toInt();
+    if (version < minSupportedVersion) {
+        errorString = QObject::tr("File version %1 is no longer supported").arg(version);
+        return false;
+    }
+    if (version > maxSupportedVersion) {
+        errorString = QObject::tr("File version %1 is newer than current supported version %2").arg(version).arg(maxSupportedVersion);
+        return false;
+    }
+
+    return true;
+}
+
+bool JsonHelper::validateExternalQGCJsonFile(const QJsonObject& jsonObject,
+                                             const QString&     expectedFileType,
+                                             int                minSupportedVersion,
+                                             int                maxSupportedVersion,
+                                             int&               version,
+                                             QString&           errorString)
+{
+    // Validate required keys
+    QList<JsonHelper::KeyValidateInfo> requiredKeys = {
+        { jsonGroundStationKey, QJsonValue::String, true },
+    };
+    if (!JsonHelper::validateKeys(jsonObject, requiredKeys, errorString)) {
+        return false;
+    }
+
+    return validateInternalQGCJsonFile(jsonObject, expectedFileType, minSupportedVersion, maxSupportedVersion, version, errorString);
+}
+
+QStringList JsonHelper::_addDefaultLocKeys(QJsonObject& jsonObject)
+{
+    QString translateKeys;
+    QString fileType = jsonObject[jsonFileTypeKey].toString();
+    if (!fileType.isEmpty()) {
+        if (fileType == MissionCommandList::qgcFileType) {
+            if (jsonObject.contains(_translateKeysKey)) {
+                translateKeys = jsonObject[_translateKeysKey].toString();
+            } else {
+                translateKeys = "label,enumStrings,friendlyName,description,category";
+                jsonObject[_translateKeysKey] = translateKeys;
+            }
+            if (!jsonObject.contains(_arrayIDKeysKey)) {
+                jsonObject[_arrayIDKeysKey] = "rawName,comment";
+            }
+        } else if (fileType == FactMetaData::qgcFileType) {
+            if (jsonObject.contains(_translateKeysKey)) {
+                translateKeys = jsonObject[_translateKeysKey].toString();
+            } else {
+                translateKeys = "shortDescription,longDescription,enumStrings";
+                jsonObject[_translateKeysKey] = "shortDescription,longDescription,enumStrings";
+            }
+            if (!jsonObject.contains(_arrayIDKeysKey)) {
+                jsonObject[_arrayIDKeysKey] = "name";
+            }
+        }
+    }
+    return translateKeys.split(",");
+}
+
+QJsonObject JsonHelper::_translateObject(QJsonObject& jsonObject, const QString& translateContext, const QStringList& translateKeys)
+{
+    for (const QString& key: jsonObject.keys()) {
+        if (jsonObject[key].isString()) {
+            QString locString = jsonObject[key].toString();
+            if (translateKeys.contains(key)) {
+                QString disambiguation;
+                QString disambiguationPrefix("#loc.disambiguation#");
+
+                if (locString.startsWith(disambiguationPrefix)) {
+                    locString = locString.right(locString.length() - disambiguationPrefix.length());
+                    int commentEndIndex = locString.indexOf("#");
+                    if (commentEndIndex != -1) {
+                        disambiguation = locString.left(commentEndIndex);
+                        locString = locString.right(locString.length() - disambiguation.length() - 1);
+                    }
+                }
+
+                QString xlatString = qgcApp()->qgcJSONTranslator().translate(translateContext.toUtf8().constData(), locString.toUtf8().constData(), disambiguation.toUtf8().constData());
+                if (!xlatString.isNull()) {
+                    jsonObject[key] = xlatString;
+                }
+            }
+        } else if (jsonObject[key].isArray()) {
+            QJsonArray childJsonArray = jsonObject[key].toArray();
+            jsonObject[key] = _translateArray(childJsonArray, translateContext, translateKeys);
+        } else if (jsonObject[key].isObject()) {
+            QJsonObject childJsonObject = jsonObject[key].toObject();
+            jsonObject[key] = _translateObject(childJsonObject, translateContext, translateKeys);
+        }
+    }
+
+    return jsonObject;
+}
+
+QJsonArray JsonHelper::_translateArray(QJsonArray& jsonArray, const QString& translateContext, const QStringList& translateKeys)
+{
+    for (int i=0; i<jsonArray.count(); i++) {
+        QJsonObject childJsonObject = jsonArray[i].toObject();
+        jsonArray[i] = _translateObject(childJsonObject, translateContext, translateKeys);
+    }
+
+    return jsonArray;
+}
+
+QJsonObject JsonHelper::_translateRoot(QJsonObject& jsonObject, const QString& translateContext, const QStringList& translateKeys)
+{
+    return _translateObject(jsonObject, translateContext, translateKeys);
+}
+
+QJsonObject JsonHelper::openInternalQGCJsonFile(const QString&  jsonFilename,
+                                                const QString&  expectedFileType,
+                                                int             minSupportedVersion,
+                                                int             maxSupportedVersion,
+                                                int             &version,
+                                                QString&        errorString)
+{
+    QFile jsonFile(jsonFilename);
+    if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
+        errorString = tr("Unable to open file: '%1', error: %2").arg(jsonFilename).arg(jsonFile.errorString());
+        return QJsonObject();
+    }
+
+    QByteArray bytes = jsonFile.readAll();
+    jsonFile.close();
+    QJsonParseError jsonParseError;
+    QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
+    if (jsonParseError.error != QJsonParseError::NoError) {
+        errorString = tr("Unable to parse json file: %1 error: %2 offset: %3").arg(jsonFilename).arg(jsonParseError.errorString()).arg(jsonParseError.offset);
+        return QJsonObject();
+    }
+
+    if (!doc.isObject()) {
+        errorString = tr("Root of json file is not object: %1").arg(jsonFilename);
+        return QJsonObject();
+    }
+
+    QJsonObject jsonObject = doc.object();
+    bool success = validateInternalQGCJsonFile(jsonObject, expectedFileType, minSupportedVersion, maxSupportedVersion, version, errorString);
+    if (!success) {
+        errorString = tr("Json file: '%1'. %2").arg(jsonFilename).arg(errorString);
+        return QJsonObject();
+    }
+
+    QStringList translateKeys = _addDefaultLocKeys(jsonObject);
+    QString context = QFileInfo(jsonFile).fileName();
+    return _translateRoot(jsonObject, context, translateKeys);
+}
+
+void JsonHelper::saveQGCJsonFileHeader(QJsonObject&     jsonObject,
+                                       const QString&   fileType,
+                                       int              version)
+{
+    jsonObject[jsonGroundStationKey] = jsonGroundStationValue;
+    jsonObject[jsonFileTypeKey] = fileType;
+    jsonObject[jsonVersionKey] = version;
+}
+
+bool JsonHelper::loadGeoCoordinateArray(const QJsonValue&   jsonValue,
+                                        bool                altitudeRequired,
+                                        QVariantList&       rgVarPoints,
+                                        QString&            errorString)
+{
+    if (!jsonValue.isArray()) {
+        errorString = QObject::tr("value for coordinate array is not array");
+        return false;
+    }
+    QJsonArray rgJsonPoints = jsonValue.toArray();
+
+    rgVarPoints.clear();
+    for (int i=0; i<rgJsonPoints.count(); i++) {
+        QGeoCoordinate coordinate;
+
+        if (!JsonHelper::loadGeoCoordinate(rgJsonPoints[i], altitudeRequired, coordinate, errorString)) {
+            return false;
+        }
+        rgVarPoints.append(QVariant::fromValue(coordinate));
+    }
+
+    return true;
+}
+
+bool JsonHelper::loadGeoCoordinateArray(const QJsonValue&       jsonValue,
+                                        bool                    altitudeRequired,
+                                        QList<QGeoCoordinate>&  rgPoints,
+                                        QString&                errorString)
+{
+    QVariantList rgVarPoints;
+
+    if (!loadGeoCoordinateArray(jsonValue, altitudeRequired, rgVarPoints, errorString)) {
+        return false;
+    }
+
+    rgPoints.clear();
+    for (int i=0; i<rgVarPoints.count(); i++) {
+        rgPoints.append(rgVarPoints[i].value<QGeoCoordinate>());
+    }
+
+    return true;
+}
+
+void JsonHelper::saveGeoCoordinateArray(const QVariantList& rgVarPoints,
+                                        bool                writeAltitude,
+                                        QJsonValue&         jsonValue)
+{
+    QJsonArray rgJsonPoints;
+
+    // Add all points to the array
+    for (int i=0; i<rgVarPoints.count(); i++) {
+        QJsonValue jsonPoint;
+
+        JsonHelper::saveGeoCoordinate(rgVarPoints[i].value<QGeoCoordinate>(), writeAltitude, jsonPoint);
+        rgJsonPoints.append(jsonPoint);
+    }
+
+    jsonValue = rgJsonPoints;
+}
+
+void JsonHelper::saveGeoCoordinateArray(const QList<QGeoCoordinate>&    rgPoints,
+                                        bool                            writeAltitude,
+                                        QJsonValue&                     jsonValue)
+{
+    QVariantList rgVarPoints;
+
+    for (int i=0; i<rgPoints.count(); i++) {
+        rgVarPoints.append(QVariant::fromValue(rgPoints[i]));
+    }
+    return saveGeoCoordinateArray(rgVarPoints, writeAltitude, jsonValue);
+}
+
+bool JsonHelper::validateKeys(const QJsonObject& jsonObject, const QList<JsonHelper::KeyValidateInfo>& keyInfo, QString& errorString)
+{
+    QStringList             keyList;
+    QList<QJsonValue::Type> typeList;
+
+    for (int i=0; i<keyInfo.count(); i++) {
+        if (keyInfo[i].required) {
+            keyList.append(keyInfo[i].key);
+        }
+    }
+    if (!validateRequiredKeys(jsonObject, keyList, errorString)) {
+        return false;
+    }
+
+    keyList.clear();
+    for (int i=0; i<keyInfo.count(); i++) {
+        keyList.append(keyInfo[i].key);
+        typeList.append(keyInfo[i].type);
+    }
+    return validateKeyTypes(jsonObject, keyList, typeList, errorString);
+}
+
+QString JsonHelper::_jsonValueTypeToString(QJsonValue::Type type)
+{
+    const struct {
+        QJsonValue::Type    type;
+        const char*         string;
+    } rgTypeToString[] = {
+    { QJsonValue::Null,         "NULL" },
+    { QJsonValue::Bool,         "Bool" },
+    { QJsonValue::Double,       "Double" },
+    { QJsonValue::String,       "String" },
+    { QJsonValue::Array,        "Array" },
+    { QJsonValue::Object,       "Object" },
+    { QJsonValue::Undefined,    "Undefined" },
+};
+
+    for (size_t i=0; i<sizeof(rgTypeToString)/sizeof(rgTypeToString[0]); i++) {
+        if (type == rgTypeToString[i].type) {
+            return rgTypeToString[i].string;
+        }
+    }
+
+    return QObject::tr("Unknown type: %1").arg(type);
+}
+
+bool JsonHelper::loadPolygon(const QJsonArray& polygonArray, QmlObjectListModel& list, QObject* parent, QString& errorString)
+{
+    for (int i=0; i<polygonArray.count(); i++) {
+        const QJsonValue& pointValue = polygonArray[i];
+
+        QGeoCoordinate pointCoord;
+        if (!JsonHelper::loadGeoCoordinate(pointValue, false /* altitudeRequired */, pointCoord, errorString, true)) {
+            list.clearAndDeleteContents();
+            return false;
+        }
+        list.append(new QGCQGeoCoordinate(pointCoord, parent));
+    }
+
+    return true;
+}
+
+void JsonHelper::savePolygon(QmlObjectListModel& list, QJsonArray& polygonArray)
+{
+    for (int i=0; i<list.count(); i++) {
+        QGeoCoordinate vertex = list.value<QGCQGeoCoordinate*>(i)->coordinate();
+
+        QJsonValue jsonValue;
+        JsonHelper::saveGeoCoordinate(vertex, false /* writeAltitude */, jsonValue);
+        polygonArray.append(jsonValue);
+    }
+}
+
+double JsonHelper::possibleNaNJsonValue(const  QJsonValue& value)
+{
+    if (value.type() == QJsonValue::Null) {
+        return std::numeric_limits<double>::quiet_NaN();
+    } else {
+        return value.toDouble();
+    }
+}
diff --git a/src/ViewLink.h b/src/ViewLink.h
new file mode 100644
index 0000000..bb0d994
--- /dev/null
+++ b/src/ViewLink.h
@@ -0,0 +1,822 @@
+
+
+/**
+ * @file ViewLink.h
+ * @brief ViewLink SDK header file
+ * @details This header file defines ViewLink SDK interfaces and the necessary data structures
+ * @author Edwin
+ * @version 3.0.7
+ * @date    2020-06-28
+ * @copyright Copyright (c) 2020 Shenzhen Viewpro Technology Co., Ltd
+ */
+
+#ifndef __VIEW_LINK_H__
+#define __VIEW_LINK_H__
+
+#if (defined _WIN32) || (defined _WIN64) // windows
+	#ifdef VLK_EXPORTS
+		#define VLK_API extern "C" __declspec(dllexport)
+	#else
+		#define VLK_API extern "C" __declspec(dllimport)
+	#endif
+	#define VLK_CALL __stdcall
+#else
+    #define VLK_API
+	#define VLK_CALL
+#endif
+
+/** @name error code
+ *  @brief usually used as return value of interface function to indicate the operation success or not
+ * @{
+ */
+#define VLK_ERROR_NO_ERROR 0
+#define VLK_ERROR_INVALID_PARAM -1
+/** @} error code */
+
+/** @name ViewLink SDK constant
+ * @{
+ */
+/** the max value of yaw */
+#define VLK_YAW_MAX 180.0
+/** the min value of yaw */
+#define VLK_YAW_MIN -180.0
+/** the max value of pitch */
+#define VLK_PITCH_MAX 90.0
+/** the min value of pitch */
+#define VLK_PITCH_MIN -90.0
+
+/** the yaw max moving speed (20 degrees per second)*/
+#define VLK_MAX_YAW_SPEED 2000.0
+/** the pitch max moving speed (20 degrees per second)*/
+#define VLK_MAX_PITCH_SPEED 2000.0
+
+/** the min zoom (or focus) speed*/
+#define VLK_MIN_ZOOM_SPEED 1
+/** the max zoom (or focus) speed*/
+#define VLK_MAX_ZOOM_SPEED 8
+/** @} ViewLink SDK constant */
+
+/** @name ViewLink SDK data structures
+ * @{
+ */
+
+/**
+ * device connection type
+ */
+typedef enum _VLK_CONN_TYPE
+{
+    VLK_CONN_TYPE_SERIAL_PORT = 0x00, ///< serial port
+    VLK_CONN_TYPE_TCP = 0x01, ///< tcp
+    VLK_CONN_TYPE_UDP = 0x02, ///< udp, not currently supported
+	VLK_CONN_TYPE_BUTT
+}VLK_CONN_TYPE;
+
+/**
+ * TCP connect information
+ */
+typedef struct _VLK_DEV_IPADDR VLK_DEV_IPADDR;
+struct _VLK_DEV_IPADDR
+{
+    char szIPV4[16]; ///< device ipv4 address, e.g: 192.168.2.119
+    int iPort; ///< tcp port, e.g: 2000
+};
+
+/**
+ * serial port connect information
+ */
+typedef struct _VLK_DEV_SERIAL_PORT VLK_DEV_SERIAL_PORT;
+struct _VLK_DEV_SERIAL_PORT
+{
+    char szSerialPortName[16]; ///< serial port name, e.g: /dev/ttyS0 on linux or COM1 on windows
+    int iBaudRate; ///< baudrate, e.g: 115200
+};
+
+/**
+ * device connect parameter
+ */
+typedef struct _VLK_CONN_PARAM VLK_CONN_PARAM;
+struct _VLK_CONN_PARAM
+{
+	VLK_CONN_TYPE emType;
+	union {
+		VLK_DEV_IPADDR IPAddr;
+		VLK_DEV_SERIAL_PORT SerialPort;
+	} ConnParam;
+};
+
+/**
+ * device connection status
+ */
+typedef enum _VLK_CONN_STATUS
+{
+    VLK_CONN_STATUS_CONNECTED = 0x00, ///< deprecated
+    VLK_CONN_STATUS_DISCONNECTED = 0x01, ///< deprecated
+    VLK_CONN_STATUS_TCP_CONNECTED = 0x02, ///< TCP connected
+    VLK_CONN_STATUS_TCP_DISCONNECTED = 0x03, ///< TCP disconnected
+    VLK_CONN_STATUS_SERIAL_PORT_CONNECTED = 0x04, ///< serial port connected
+    VLK_CONN_STATUS_SERIAL_PORT_DISCONNECTED = 0x05, ///< serial port disconnectd
+	VLK_CONN_STATUS_BUTT
+}VLK_CONN_STATUS;
+
+/**
+ * track template size
+ */
+typedef enum _VLK_TRACK_TEMPLATE_SIZE
+{
+	VLK_TRACK_TEMPLATE_SIZE_AUTO = 0,
+	VLK_TRACK_TEMPLATE_SIZE_32 = 32,
+	VLK_TRACK_TEMPLATE_SIZE_64 = 64,
+	VLK_TRACK_TEMPLATE_SIZE_128 = 128,
+	VLK_TRACK_TEMPLATE_SIZE_BUTT
+}VLK_TRACK_TEMPLATE_SIZE;
+
+/**
+ * sensor type
+ */
+typedef enum _VLK_SENSOR
+{
+    VLK_SENSOR_VISIBLE1 = 0, ///< visible light only
+    VLK_SENSOR_IR = 1, ///< IR only
+    VLK_SENSOR_VISIBLE_IR = 2, ///< visible light + IR PIP
+    VLK_SENSOR_IR_VISIBLE = 3, ///< IR + visible light PIP
+    VLK_SENSOR_VISIBLE2 = 4, ///< prime lens visible light
+	VLK_SENSOR_BUTT
+}VLK_SENSOR;
+
+/**
+ * track mode parameter
+ */
+typedef struct _VLK_TRACK_MODE_PARAM VLK_TRACK_MODE_PARAM;
+struct _VLK_TRACK_MODE_PARAM
+{
+	VLK_TRACK_TEMPLATE_SIZE emTrackTempSize;
+	VLK_SENSOR emTrackSensor;
+};
+
+/**
+ * Record mode
+ */
+typedef enum _VLK_RECORD_MODE
+{
+    VLK_RECORD_MODE_NONE = 0, ///< none mode, neither photo nor record mode
+    VLK_RECORD_MODE_PHOTO = 1, ///< photo mode, take photos only
+    VLK_RECORD_MODE_RECORD = 2, ///< record mode, video record only
+	VLK_RECORD_MODE_BUTT
+}VLK_RECORD_MODE;
+
+/**
+ * Focus mode
+ */
+typedef enum _VLK_FOCUS_MODE
+{
+    VLK_FOCUS_MODE_AUTO = 0, ///< automatic focus
+    VLK_FOCUS_MODE_MANU = 1, ///< manual focus
+	VLK_FOCUS_MODE_BUTT
+}VLK_FOCUS_MODE;
+
+/**
+ * Laser zoom mode
+ */
+typedef enum _VLK_LASER_ZOOM_MODE
+{
+    VLK_LASER_ZOOM_MODE_FOLLOW_EO = 0, ///< follow visible light zoom
+    VLK_LASER_ZOOM_MODE_MANU = 1, ///< manual control
+	VLK_LASER_ZOOM_MODE_BUTT
+}VLK_LASER_ZOOM_MODE;
+
+/**
+ * OSD mask
+ */
+typedef enum _VLK_OSD_MASK
+{
+    VLK_OSD_MASK_ENABLE_OSD = 0x1, ///< enable OSD
+    VLK_OSD_MASK_CROSS = 0x2, ///< enable cross frame in center of image
+    VLK_OSD_MASK_PITCH_YAW = 0x4, ///< show pitch and yaw
+    VLK_OSD_MASK_XYSHIFT = 0x8, ///< enable xy shift
+    VLK_OSD_MASK_GPS = 0x10, ///< enable GPS
+    VLK_OSD_MASK_TIME = 0x20, ///< enable time
+    VLK_OSD_MASK_VL_MAG = 0x40, ///< enable VL-MAG
+    VLK_OSD_MASK_BIG_FONT = 0x80 ///< enable big font
+}VLK_OSD_MASK;
+
+/**
+ * OSD input mask
+ */
+typedef enum _VLK_OSD_INPUT_MASK
+{
+    VLK_OSD_INPUT_MASK_PERMANENT_SAVE = 0x1, ///< save OSD configuration, still work after reboot
+    VLK_OSD_INPUT_MASK_TIME = 0x2, ///< enalbe input time
+    VLK_OSD_INPUT_MASK_GPS = 0x4, ///< enable input GPS
+    VLK_OSD_INPUT_MASK_MGRS = 0x8,///< enable input MGRS
+    VLK_OSD_INPUT_MASK_PITCH_YAW = 0x10, ///< enable input pitch and yaw
+    VLK_OSD_INPUT_MASK_VL_MAG = 0x20, ///< enalbe input VL-MAG
+    VLK_OSD_INPUT_MASK_ZOOM_TIMES_OR_FOV = 0x40, ///< display zoom times or FOV
+    VLK_OSD_INPUT_MASK_CHAR_BLACK_BORDER = 0x80 ///< enable character black frame
+}VLK_OSD_INPUT_MASK;
+
+/**
+ * OSD configuration parameter
+ */
+typedef struct _VLK_OSD_PARAM VLK_OSD_PARAM;
+struct _VLK_OSD_PARAM
+{
+    char cOSD; ///< mask of VLK_OSD_MASK, e.g: VLK_OSD_MASK_ENABLE_OSD | VLK_OSD_MASK_CROSS
+    char cOSDInput; ///< mask of VLK_OSD_INPUT_MASK, e.g: VLK_OSD_INPUT_MASK_TIME | VLK_OSD_INPUT_MASK_GPS
+};
+
+/**
+ * Image type
+ */
+typedef enum _VLK_IMAGE_TYPE
+{
+    VLK_IMAGE_TYPE_VISIBLE1, ///< visible light zoom lens
+    VLK_IMAGE_TYPE_VISIBLE2, ///< visible light prime lens
+    VLK_IMAGE_TYPE_IR1, ///< IR zoom lens
+    VLK_IMAGE_TYPE_IR2, ///< IR prime lens
+    VLK_IMAGE_TYPE_FUSION, ///< visible light + IR fusion
+	VLK_IMAGE_TYPE_BUTT
+}VLK_IMAGE_TYPE;
+
+/**
+ * IR color type
+ */
+typedef enum _VLK_IR_COLOR
+{
+	VLK_IR_COLOR_WHITEHOT,
+	VLK_IR_COLOR_BLACKHOT,
+	VLK_IR_COLOR_PSEUDOHOT,
+	VLK_IR_COLOR_RUSTY,
+	VLK_IR_COLOR_BUTT
+}VLK_IR_COLOR;
+
+/**
+ * 2.4G wireless control channels map
+ */
+typedef struct _VLK_CHANNELS_MAP VLK_CHANNELS_MAP;
+struct _VLK_CHANNELS_MAP
+{
+    char cYW; ///< high 4 bits save yaw left channel index, low 4 bits save yaw right channel index
+    char cPT; ///< high 4 bits save pitch up channel index, low 4 bits save pitch down channel index
+    char cMO; ///< high 4 bits save adjust speed channel index, low 4 bits save recenter channel index
+    char cZM; ///< high 4 bits save zoom out channel index, low 4 bits save zoom in channel index
+    char cFC; ///< high 4 bits save focus out channel index, low 4 bits save focus in channel index
+    char cRP; ///< high 4 bits save take photo channel index, low 4 bits save Gimbal record channel index
+    char cMU; ///< high 4 bits save start track channel index, low 4 bits save stop track channel index
+};
+
+/**
+ * Device configuration
+ */
+typedef struct _VLK_DEV_CONFIG VLK_DEV_CONFIG;
+struct _VLK_DEV_CONFIG
+{
+    char cTimeZone; ///< time zone, e.g: 8 (Beijing), 9 (Seoul)
+    char cOSDCfg; ///< OSD configuration, equivalent to cOSD in VLK_OSD_PARAM
+    char cMagneticVariation; ///< Magnetic Variation set https://skyvector.com/   http://www.magnetic-declination.com/
+    char cOSDInput; ///< OSD input configuration, equivalent to cOSDInput in VLK_OSD_PARAM
+
+    /**
+     * @brief Gimbal serial port baudrate \n
+     * 0: 2400 \n
+     * 1: 4800 \n
+     * 2: 9600 \n
+     * 3: 19200 \n
+     * 4: 38400 \n
+     * 5: 57600 \n
+     * 6: 115200 \n
+     * 7: S.BUS mode
+     */
+    char cBaudRate;
+
+    char cEODigitalZoom; ///< EO digital zoom, 1: on; 0: off
+    short sTemperatureAlarmLine;///< temperature alarm, low 8 bits save lower limit, high 8 bits save upper limit
+    char cTrack; ///< track status, 1: enabled, 0: disabled
+
+    /**
+     * @brief laser work mode \n
+     * 0: stop LRF \n
+     * 1: 1HZ get LRF data \n
+     * 2: continuosly get LRF \n
+     * 3: one time get LRF data
+     */
+    char cLaser;
+
+    char cRecordDefinition; ///< record definition, 1: 4k 25fps; 2: 1080P 25fps
+    char cOSDGPS; ///< OSD GPS, 0: OSD GPS is UAV; 1: OSD GPS is target
+
+    /**
+     * @brief s.bus/mavlink channels map set \n
+     * 1: 1~7 \n
+     * 2: 6~12 \n
+     * 3: 8~14 \n
+     * 4: custom channels \n
+     */
+    char cSBUSChnlMap;
+
+    VLK_CHANNELS_MAP ChnlsMap; ///< custom channels map
+    char cFocusHoldSet; ///< deprecated
+    char cCameraType; ///< deprecated
+    char cReserved1[5]; ///< reserved
+    char cRestoreIP; ///< deprecated
+    char cReserved2[5]; ///< reserved
+    char cReserved3[43]; ///< reserved
+    char cVersionNO[20]; ///< firmware version NO
+    char cDeviceID[10]; ///< device model
+    char cSerialNO[22]; ///< serial NO
+};
+
+
+/**
+ * Device model
+ */
+typedef struct _VLK_DEV_MODEL VLK_DEV_MODEL;
+struct _VLK_DEV_MODEL
+{
+    char cModelCode; ///< Gimbal model code
+    char szModelName[32]; ///< Gimbal model name
+};
+
+/**
+ * Tracker status
+ */
+typedef enum _VLK_TRACKER_STATUS
+{
+	VLK_TRACKER_STATUS_STOPPED,
+	VLK_TRACKER_STATUS_SEARCHING,
+	VLK_TRACKER_STATUS_TRACKING,
+	VLK_TRACKER_STATUS_LOST,
+	VLK_TRACKER_STATUS_BUTT
+}VLK_TRACKER_STATUS;
+
+/**
+ * Device telemetry
+ */
+typedef struct _VLK_DEV_TELEMETRY VLK_DEV_TELEMETRY;
+struct _VLK_DEV_TELEMETRY
+{
+    double dYaw; ///< Gimbal current yaw
+    double dPitch; ///< Gimbal current pitch
+    VLK_SENSOR emSensorType; ///< sensor type
+    VLK_TRACKER_STATUS emTrackerStatus; ///< tracker status
+    double dTargetLat; ///< target latitude
+    double dTargetLng; ///< target longitude
+    double dTargetAlt; ///< target altitude
+    double dZoomMagTimes; ///< camera magnification times
+    short sLaserDistance; ///< laser distance
+    VLK_IR_COLOR emIRColor; ///< IR color
+    VLK_RECORD_MODE emRecordMode; ///< record mode
+};
+
+/**
+ * Device status type \n
+ * device status is passed by VLK_DevStatus_CB, \n
+ * VLK_DEV_STATUS_TYPE will be passed to formal paramter iType, \n
+ * and the device status data will be passed to szBuffer, \n
+ * different iType matches different status data struct
+ */
+typedef enum _VLK_DEV_STATUS_TYPE
+{
+    VLK_DEV_STATUS_TYPE_MODEL, ///< matches VLK_DEV_MODEL
+    VLK_DEV_STATUS_TYPE_CONFIG, ///< matches VLK_DEV_CONFIG
+    VLK_DEV_STATUS_TYPE_TELEMETRY, ///< matches VLK_DEV_TELEMETRY
+	VLK_DEV_STATUS_TYPE_BUTT,
+}VLK_DEV_STATUS_TYPE;
+/** @} ViewLink SDK data structures */
+
+/** @name ViewLink SDK callback functions
+ * @{
+ */
+/** @brief connection status callback
+ *  @param iConnStatus connection status, it must be one of VLK_CONN_STATUS
+ *  @param szMessage extended string message
+ *  @param iMsgLen extended string message length
+ *  @return return 0 anyway
+ *  @details VLK_ConnStatus_CB will be called by SDK once the connection status changed,
+ *           for example, developers should check connect result in this callback function
+ * @see VLK_Connect
+ */
+typedef int (*VLK_ConnStatus_CB)(int iConnStatus, const char* szMessage, int iMsgLen, void* pUserParam);
+
+/** @brief device status callback
+ *  @param iType device status type, it must be one of VLK_DEV_STATUS_TYPE
+ *  @param szBuffer device status data, it is an address of a struct matchs iType, e.g: VLK_DEV_MODEL
+ *  @param iBufLen device status data length, it must be equal to the size of struct matchs iType, e.g: iBufLen == sizeof(VLK_DEV_MODEL)
+ *  @return return 0 anyway
+ *  @details VLK_DevStatus_CB will be called by SDK once device new status is received, developers can easily get Gimbal latest status
+ *  @see VLK_RegisterDevStatusCB
+ */
+typedef int (*VLK_DevStatus_CB)(int iType, const char* szBuffer, int iBufLen, void* pUserParam);
+/** @} ViewLink SDK callback functions */
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Get SDK version
+ *  @return SDK version number, e.g: 3.0.6
+ *  @details we recommend printing the SDK version number before you use SDK,
+ *           this is the only interface can be called before VLK_Init()
+ */
+VLK_API	const char* VLK_CALL    GetSDKVersion();
+
+/** @brief Initialize SDK
+ *  @return VLK_ERROR_NO_ERROR on success, error code otherwise
+ *  @details it should be called somewhere at the beginning of your application,
+ *           and it should be called just once during the lifecirle of your application,
+ *           all interfaces are unavailbe before initialized
+ */
+VLK_API	int     VLK_CALL    VLK_Init();
+
+/** @brief Uninitialize SDK
+ *  @details it should be called somewhere at the end of your application,
+ *           and it should be called just once during the lifecirle of your application,
+ *           once you uninitialize sdk, all interfaces are nomore available
+ */
+VLK_API	void    VLK_CALL    VLK_UnInit();
+
+/** @brief connect Gimbal with specific type, TCP and serial port are available
+ *  @param pConnParam connect information, including connection type (TCP or serial port), ip address, port, serial port name, baudrate
+ *  @param pConnStatusCB  connnection status callback function, it will be called by SDK when device is connected or disconnected
+ *  @param pUserParam user parameters, will be passed back when pConnStatusCB is called
+ *  @return VLK_ERROR_NO_ERROR on success, error code otherwise
+ *  @details this function is asynchronous, the connect result will be passed back by callback function, do not use return value to judge connect result.
+ *           SDK allows only one connection, it means if you create a new connection, the previous one will be disconnected automatically
+ */
+VLK_API	int     VLK_CALL    VLK_Connect(const VLK_CONN_PARAM* pConnParam, VLK_ConnStatus_CB pConnStatusCB, void* pUserParam);
+
+/** @brief Check if Gimbal is connected
+ *  @return
+ *  @retval 1 if either TCP or serial port is connected
+ *  @retval 0 if neither TCP nor serial port is connected
+ */
+VLK_API	int     VLK_CALL    VLK_IsConnected();
+
+/** @brief Check if Gimbal is TCP connected
+ *  @return
+ *  @retval 1 if TCP is connected
+ *  @retval 0 if TCP is disconnected
+ */
+VLK_API	int     VLK_CALL    VLK_IsTCPConnected();
+
+/** @brief Check if Gimbal is serial port connected
+ *  @return
+ *  @retval 1 if serial port is connected
+ *  @retval 0 if serial port is disconnected
+ */
+VLK_API	int     VLK_CALL    VLK_IsSerialPortConnected();
+
+/** @brief Disconnect current connection no matter it is TCP, serial port or other
+ */
+VLK_API	void	VLK_CALL    VLK_Disconnect();
+
+/** @brief Disconnect current TCP connection
+ */
+VLK_API	void	VLK_CALL    VLK_DisconnectTCP();
+
+/** @brief Disconnect current serial port connection
+ */
+VLK_API	void	VLK_CALL    VLK_DisconnectSerialPort();
+
+/** @brief Set keep alive interval
+ *  @param iMS keep alive interval in milliseconds range from 100 ms to 5000 ms
+ *  @details keep alive interval determines the frequncy of querying Gimbal telemetry data,
+ *           SDK default keep alive interval is 500 ms, no need to change it if not necessary
+ */
+VLK_API void	VLK_CALL    VLK_SetKeepAliveInterval(int iMS);
+
+/** @brief Register device status callback
+ *  @param pDevStatusCB receive device status callback function
+ *  @param pUserParam user parameter, it will be passed back when pDevStatusCB is called
+ *  @details pDevStatusCB will be called once SDK received new Gimbal,
+ *           specifically, the telemetry data will keep updating once device is connected,
+ *           ignore it if you don't need it
+ */
+VLK_API	void	VLK_CALL    VLK_RegisterDevStatusCB(VLK_DevStatus_CB pDevStatusCB, void* pUserParam);
+
+/** @brief Control Gimbal yaw and pitch
+ *  @param sHorizontalSpeed the speed of changing yaw (0.01 degrees/s), for example, 2000 means 20 degrees per second
+ *  @param sVeritcalSpeed the speed of changing pitch (0.01 degrees/s)
+ *  @details considering a small angle adjustment will result in a huge visual field changing in sky view, we limit
+ *           speed in a proper range: \n
+ *           -VLK_MAX_YAW_SPEED <= sHorizontalSpeed <= VLK_MAX_YAW_SPEED \n
+ *           -VLK_MAX_PITCH_SPEED <= sVeritcalSpeed <= VLK_MAX_PITCH_SPEED \n
+ *           some example: \n
+ *           move up: VLK_Move(0, 1000); \n
+ *           move left: VLK_Move(-1000, 0); \n
+ *           move right: VLK_Move(1000, 0); \n
+ *           move down: VLK_Move(0, -1000);
+ */
+VLK_API	void	VLK_CALL    VLK_Move(short sHorizontalSpeed, short sVeritcalSpeed);
+
+/** @brief Stop moving
+ *  @details once you call VLK_Move, Gimbal will keep moving until it reach the yaw and pitch limitation or VLK_Stop is called
+ */
+VLK_API	void	VLK_CALL    VLK_Stop();
+
+/** @brief Zoom in
+ *  @param sSpeed the speed of zoom in
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomIn(short sSpeed);
+
+/** @brief Zoom out
+ *  @param sSpeed the speed of zoom out
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomOut(short sSpeed);
+
+/** @brief Stop zoom
+ *  @details once you call VLK_ZoomIn or VLK_ZoomOut, Camera will keep zooming until it reach the limitation or VLK_StopZoom is called
+ */
+VLK_API	void	VLK_CALL    VLK_StopZoom();
+
+/** @brief Enable track mode
+ *  @param Param track mode parameters
+ *  @see VLK_TrackTargetPositionEx
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_EnableTrackMode(const VLK_TRACK_MODE_PARAM* pParam);
+
+/** @brief Track target by it's position
+ *  @param iX target position, number of pixels from top-left corner in horizontal direction
+ *  @param iY taget position, number of  pixels from top-left corner in vertical direction
+ *  @param iVideoWidth video image width, e.g: 1920, 1280
+ *  @param iVideoHeight video image height, e.g: 1080, 720
+ *  @deprecated
+ *  @see VLK_TrackTargetPositionEx
+ */
+VLK_API	void	VLK_CALL    VLK_TrackTargetPosition(int iX, int iY, int iVideoWidth, int iVideoHeight);
+
+/** @brief Track target by it's position
+ *  @param pParam track mode parameters,
+ *  @param iX target position, number of pixels from top-left corner in horizontal direction
+ *  @param iY taget position, number of  pixels from top-left corner in vertical direction
+ *  @param iVideoWidth video image width, e.g: 1920, 1280
+ *  @param iVideoHeight video image height, e.g: 1080, 720
+ *  @details we usually use VLK_TRACK_TEMPLATE_SIZE_AUTO for pParam->emTrackTempSize and VLK_SENSOR_VISIBLE1 for pParam->emTrackSensor
+ */
+VLK_API	void	VLK_CALL    VLK_TrackTargetPositionEx(const VLK_TRACK_MODE_PARAM* pParam, int iX, int iY, int iVideoWidth, int iVideoHeight);
+
+/** @brief Disable track mode
+ */
+VLK_API	void	VLK_CALL    VLK_DisableTrackMode();
+
+/** @brief Focus in
+ *  @param sSpeed the speed of focus in
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ *           this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to
+ *           get current focus mode, call VLK_SetFocusMode to set focus mode
+ */
+VLK_API	void	VLK_CALL    VLK_FocusIn(short sSpeed);
+
+/** @brief Focus out
+ *  @param sSpeed the speed of focus out
+ *  @details sSpeed must be a short number from VLK_MIN_ZOOM_SPEED to VLK_MAX_ZOOM_SPEED, the bigger the faster
+ *           this function is available only when the Gimbal is in Manual focusing mode, call VLK_GetFocusMode to
+ *           get current focus mode, call VLK_SetFocusMode to set focus mode
+ */
+VLK_API	void	VLK_CALL    VLK_FocusOut(short sSpeed);
+
+/** @brief Stop focus
+ *  @details  once you call VLK_FocusIn or VLK_FocusOut, Camera will keep focusing until it reach the limitation or VLK_StopFocus is called
+ */
+VLK_API	void	VLK_CALL    VLK_StopFocus();
+
+/** @brief Move to home position
+ */
+VLK_API	void	VLK_CALL    VLK_Home();
+
+/** @brief Switch motor
+ *  @param iOn turn on/turn off \n
+ *         1 turn on \n
+ *         0 turn off
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchMotor(int iOn);
+
+/** @brief Check motor status
+ *  @return current motor status
+ *  @retval 1 on
+ *  @retval 0 off
+ */
+VLK_API	int     VLK_CALL    VLK_IsMotorOn();
+
+/** @brief Enable Gimbal follow drone
+ *  @param iEnable enable or disable \n
+ *         1 enable \n
+ *         0 disable
+ */
+VLK_API	void	VLK_CALL    VLK_EnableFollowMode(int iEnable);
+
+/** @brief Check follow mode
+ *  @return current follow status
+ *  @retval 1 enabled
+ *  @retval 0 disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsFollowMode();
+
+/** @brief Turn to specific yaw and pitch
+ *  @param dYaw specific yaw, must be a double value from VLK_YAW_MIN to VLK_YAW_MAX
+ *  @param dPitch specific pitch, must be a double value from VLK_PITCH_MIN to VLK_PITCH_MAX
+ */
+VLK_API	void	VLK_CALL    VLK_TurnTo(double dYaw, double dPitch);
+
+/** @brief Enable track
+ */
+VLK_API	void	VLK_CALL    VLK_StartTrack();
+
+
+/** @brief Disable track
+ */
+VLK_API	void	VLK_CALL    VLK_StopTrack();
+
+/** @brief Check if track is enabled
+ *  @return current track status
+ *  @retval 1 enabled
+ *  @retval 0 disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsTracking();
+
+/** @brief Set track template size
+ *  @param emSize track template size enumeration
+ *  @see VLK_TRACK_TEMPLATE_SIZE
+ */
+VLK_API	void	VLK_CALL    VLK_SetTrackTemplateSize(VLK_TRACK_TEMPLATE_SIZE emSize);
+
+/** @brief Set image color
+ *  @param emImgType image type enumeration
+ *  @param iEnablePIP enable picture in picture \n
+ *         1 enable \n
+ *         0 disable
+ * @param emIRColor IR color enumeration
+ *  @see VLK_IMAGE_TYPE
+ */
+VLK_API	void	VLK_CALL    VLK_SetImageColor(VLK_IMAGE_TYPE emImgType, int iEnablePIP, VLK_IR_COLOR emIRColor);
+
+/** @brief Ask Gimbal take a photograph
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	void	VLK_CALL    VLK_Photograph();
+
+/** @brief Gimbal start or stop recording
+ *  @param iOn \n
+ *          1 start recording \n
+ *          0 stop recording
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchRecord(int iOn);
+
+/** @brief Check if Gimbal is recording
+ *  @return flag of recording status
+ *  @retval 1 recording
+ *  @retval 0 not recording
+ *  @details make sure there is SD card in the Gimbal
+ */
+VLK_API	int     VLK_CALL    VLK_IsRecording();
+
+/** @brief Enable defog
+ *  @param iOn \n
+ *          1 defog is enabled \n
+ *          0 defog is disabled
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchDefog(int iOn);
+
+/** @brief Check if defog is enabled
+ *  @return flag of defog status
+ *  @retval 1 defog is enabled
+ *  @retval 0 defog is disabled
+ */
+VLK_API	int     VLK_CALL    VLK_IsDefogOn();
+
+/** @brief Set record mode
+ *  @param emMode record mode enumeration
+ *  @details some models could not take photo while it is recording,
+ *           they must be switched to a certain
+ * @see VLK_RECORD_MODE
+ */
+VLK_API	void	VLK_CALL    VLK_SetRecordMode(VLK_RECORD_MODE emMode);
+
+/** @brief Get current record mode
+ *  @return record mode enumeration
+ *  @see VLK_RECORD_MODE
+ */
+VLK_API	int     VLK_CALL    VLK_GetRecordMode();
+
+/** @brief Set focus mode ( manual focus or automatic focus)
+ *  @param focus mode enumeration
+ *  @see VLK_FOCUS_MODE
+ */
+VLK_API	void	VLK_CALL    VLK_SetFocusMode(VLK_FOCUS_MODE emMode);
+
+/** @brief Get current focus mode ( manual focus or automatic focus)
+ *  @return focus mode enumeration
+ *  @see VLK_FOCUS_MODE
+ */
+VLK_API	int     VLK_CALL    VLK_GetFocusMode();
+
+/** @brief Zoom to a specific magnification
+ *  @param fMag specific magnification
+ *  @details the camera will reach the limitation if the specific magnification over it's capability
+ */
+VLK_API	void	VLK_CALL    VLK_ZoomTo(float fMag);
+
+/** @brief IR digital zoom in
+ *  @param sSpeed deprecated, pass 0
+ *  @details zoom in x1 then stop automatically
+ */
+VLK_API	void	VLK_CALL    VLK_IRDigitalZoomIn(short sSpeed);
+
+/** @brief IR digital zoom out
+ *  @param sSpeed deprecated, pass 0
+ *  @details zoom out x1 then stop automatically
+ */
+VLK_API	void	VLK_CALL    VLK_IRDigitalZoomOut(short sSpeed);
+
+/** @brief Switch EO digital zoom status
+ *  @param iOn \n
+ *         1 enable \n
+ *         0 disable
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchEODigitalZoom(int iOn);
+
+/** @brief Enter S-BUS mode
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_EnterSBUSMode();
+
+/** @brief Exit S-BUS mode
+ *  @deprecated
+ */
+VLK_API	void	VLK_CALL    VLK_ExitSBUSMode();
+
+/** @brief Query device configuration
+ *  @details this function is asynchronous, configuration data will be passed back throw VLK_DevStatus_CB
+ *  @see VLK_DevStatus_CB
+ */
+VLK_API	void	VLK_CALL    VLK_QueryDevConfiguration();
+
+/** @brief Set OSD
+ *  @param pParam OSG configuration
+ *  @details we don't provide VLK_GetOSD because OSD is included in device configuration
+ *  @see VLK_OSD_PARAM
+ */
+VLK_API	void	VLK_CALL    VLK_SetOSD(const VLK_OSD_PARAM* pParam);
+
+/** @brief Set wireless control channels map
+ *  @param pChnlsMap wireless control channels map configuration
+ *  @details we don't provide VLK_GetWirelessCtrlChnlMap because channels map is included in device configuration
+ *  @see VLK_CHANNELS_MAP
+ */
+VLK_API	void	VLK_CALL    VLK_SetWirelessCtrlChnlMap(const VLK_CHANNELS_MAP* pChnlsMap);
+
+/** @brief Send extent command
+ *  @param szCmd command data pointer
+ *  @param iLen command data length
+ *  @details for some seldom-used Gimbal control command which we didn't provide specific interface,
+ *           you should call this function to send command data directly
+ */
+VLK_API	void	VLK_CALL    VLK_SendExtentCmd(const char* szCmd, int iLen);
+
+
+/** @name Laser Control functions
+ * @{
+ */
+
+/** @brief Switch laser status
+ *  @param iOn \n
+ *         1 turn on \n
+ *         0 turn off
+ *  @details make sure your device has laser capability
+ */
+VLK_API	void	VLK_CALL    VLK_SwitchLaser(int iOn);
+
+/** @brief Laser single ranging
+ */
+VLK_API	void	VLK_CALL    VLK_LaserSingle();
+
+
+/** @brief Laser zoom in
+ *  @param sSpeed deprecated, pass 0
+ */
+VLK_API	void	VLK_CALL    VLK_LaserZoomIn(short sSpeed);
+
+/** @brief Laser zoom out
+ *  @param sSpeed deprecated, pass 0
+ */
+VLK_API	void	VLK_CALL    VLK_LaserZoomOut(short sSpeed);
+
+/** @brief Laser stop zoom
+ */
+VLK_API	void	VLK_CALL    VLK_LaserStopZoom();
+
+/** @brief Set laser zoom mode
+ *  @param emMode laser zoom mode enumeration
+ */
+VLK_API	void	VLK_CALL    VLK_SetLaserZoomMode(VLK_LASER_ZOOM_MODE emMode);
+/** @} Laser Control functions */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //__VIEW_LINK_H__
diff --git a/src/gimbal_base.hpp b/src/gimbal_base.hpp
new file mode 100644
index 0000000..33fe7a2
--- /dev/null
+++ b/src/gimbal_base.hpp
@@ -0,0 +1,454 @@
+#ifndef gimbal_base
+#define gimbal_base
+#include <stdint.h>
+#include <iostream>
+#include <opencv2/videoio.hpp>
+#include <opencv2/videoio/videoio_c.h>
+// #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+class GimbalBase
+{
+    friend std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal);
+
+public:
+    static std::string time_string_now();
+
+
+    // vel rpy
+    GimbalBase() = delete;
+    GimbalBase(std::string save_path = "gimbal_save_data", std::string device_name = "", int max_zoom = 0x4000, int min_zoom = 0x0000)
+        : past_clock(std::chrono::system_clock::now()), past_clock_zoom(std::chrono::system_clock::now()), max_zoom(max_zoom), min_zoom(min_zoom), save_path(save_path), device_name(device_name)
+    {
+
+        this->save_path = save_path[save_path.size() - 1] == '/' ? save_path : save_path + '/';
+        this->save_path = this->save_path[0] == '/' ? this->save_path : std::string("/") + this->save_path;
+        this->save_path = std::string(getenv("HOME")) + this->save_path;
+        std::cout << "Data Save Path: " << this->save_path << std::endl;
+        if (access(this->save_path.c_str(), 0x00) != 0)
+        {
+            mkdir(this->save_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO);
+            if (access(this->save_path.c_str(), 0x00) != 0)
+            {
+                throw std::runtime_error("Create dir for save DIR failed!");
+            }
+        }
+    };
+    GimbalBase(const GimbalBase &) = delete;
+    virtual ~GimbalBase() = default;
+    // 娲剧敓绫诲繀椤昏瀹炵幇鍒版柟娉�
+    virtual void open_tty(std::string tty_port = "/dev/ttyUSB0") = 0;
+    virtual void open_camera(std::string camera_id = "0", int width = 640, int height = 480, int fps=30) = 0;
+    virtual bool get_info(float ret[12]) = 0; // 鍏ュ彛
+    virtual void ctl_gimbal(uint8_t rpy_ctl[3], double rpy_vel[3], bool stop_search = true) = 0;
+    virtual void home() = 0;
+    virtual void hold() = 0;
+    virtual void fellow() = 0;
+
+    // 鍩虹被宸叉湁鍩虹鍔熻兘
+    virtual void tracking(float xy_info[6], int ctl_roll = 0, float kp = 0.1, float ki = 0.0, float kd = 0.0, float ignore_error = 35);
+    virtual bool search(bool is_roll, float search_vision_h = 50, float search_vision_v = 30, float search_center_h = 0, float search_center_v = 45);
+    virtual bool start_record();
+    virtual void finish_record();
+    virtual bool get_picture(std::string name = "");
+    virtual float get_dt();
+
+    // 娲剧敓绫诲彲閫�
+    virtual void focus_out(){};
+    virtual void focus_in(){};
+    virtual void focus_stop(){};
+    virtual void zoom_in(){};
+    virtual void zoom_out(){};
+    virtual void zoom_stop(){};
+    virtual uint16_t get_zoom_val(){};
+    virtual bool auto_zoom(float curr_area, uint16_t now_zoom_val, float thresh_area_up = 0.1, float thresh_area_down = 0.5){};
+
+    // 璇诲彇瑙嗛娴�
+    cv::VideoCapture &operator>>(cv::Mat &mat);
+    cv::VideoCapture cap;
+    // 淇濆瓨瑙嗛
+    cv::VideoWriter outvideo;
+    // 褰撳墠姝ｅ湪澶勭悊鐨勫抚
+    cv::Mat frame;
+
+    // 褰撴湡鐘舵��
+    enum
+    {
+        MANUAL_CONTROL = 0,
+        HOME_BACK,
+        TRACKING,
+        YAW_FOLLOW,
+        HOLDING,
+        SEARCHING
+    } move_state;
+
+    enum
+    {
+        ZOOM_KEEP = 0,
+        ZOOM_OUT,
+        ZOOM_IN,
+        ZOOM_AUTO
+    } zoom_state;
+    enum
+    {
+        INTERACTIVE_FEEDBACK = 0,
+        AUTO_FEEDBACK
+    } feedback_state;
+
+    uint16_t zoom_val = 0;
+    float fzoom_val = 0.;
+    bool is_recording = false;
+
+    std::string save_path;
+    const uint16_t max_zoom;
+    const uint16_t min_zoom;
+
+    // 鍋滄
+    bool is_stop_auto = false;
+
+    // 鍚婅埍瑙掑害淇℃伅
+    float past_stator_rel_camera[3];
+    float gimbal_state[12];
+    float gimbal_state_paste[12];
+    /*
+    std::cout << "----------------------gimbal-now-status-------------------------" << std::endl;
+    std::cout << "imu_roll:       " << std::setprecision(6) << ret[0] << std::endl;
+    std::cout << "imu_pitch:      " << std::setprecision(6) << ret[1] << std::endl;
+    std::cout << "imu_yaw:        " << std::setprecision(6) << ret[2] << std::endl;
+    std::cout << "target_roll:    " << std::setprecision(6) << ret[3] << std::endl;
+    std::cout << "target_pitch:   " << std::setprecision(6) << ret[4] << std::endl;
+    std::cout << "target_yaw:     " << std::setprecision(6) << ret[5] << std::endl;
+    std::cout << "rel_roll:       " << std::setprecision(6) << ret[6] << std::endl;
+    std::cout << "rel_pitch:      " << std::setprecision(6) << ret[7] << std::endl;
+    std::cout << "rel_yaw:        " << std::setprecision(6) << ret[8] << std::endl;
+    std::cout << "imu_roll_vel:   " << std::setprecision(6) << ret[9] << std::endl;
+    std::cout << "imu_pitch_vel:  " << std::setprecision(6) << ret[10] << std::endl;
+    std::cout << "imu_yaw_vel:    " << std::setprecision(6) << ret[11] << std::endl;
+    */
+
+    const std::string device_name;
+
+protected:
+    std::chrono::system_clock::time_point past_clock;
+    // 淇濆瓨涓婃鏌ヨ鏀惧ぇ鍊嶆暟鏃堕棿
+    std::chrono::system_clock::time_point past_clock_zoom;
+
+    int camera_width;
+    int camera_hight;
+
+    // 鐢ㄤ簬涓柇鍚婅埍鎼滅储鍒扮嚎绋�
+    bool stop_search_;
+    // 鍚婁粨鎼滅储璺嚎
+    float search_line[4][2];
+    std::thread *t_search = nullptr;
+
+    void _search(bool is_roll = false);
+    // 鐢ㄤ簬鍐嶅悐鑸辫繘琛岃浆鍔ㄦ悳绱㈡椂鍒ゅ畾锛屾槸鍚﹀埌杈炬湡鏈涘�笺��
+    // q10f鎺у埗鐨勬槸鐢垫満瑙掑害锛� g1鎺у埗鐨勬槸yaw鐢垫満瑙掑害锛宺oll,pitch鍒癷mu瑙掑害
+    double control_feedback_truth[3];
+};
+
+bool GimbalBase::search(bool is_roll, float search_vision_h, float search_vision_v, float search_center_h, float search_center_v)
+{
+    float ignore_error = 5;
+    this->search_line[0][0] = search_center_v - search_vision_v / 2 - ignore_error;
+    this->search_line[0][1] = search_center_h - search_vision_h / 2 - ignore_error;
+
+    this->search_line[1][0] = search_center_v - search_vision_v / 2 - ignore_error;
+    this->search_line[1][1] = search_center_h + search_vision_h / 2 + ignore_error;
+
+    this->search_line[2][0] = search_center_v + search_vision_v / 2 + ignore_error;
+    this->search_line[2][1] = search_center_h + search_vision_h / 2 + ignore_error;
+
+    this->search_line[3][0] = search_center_v + search_vision_v / 2 + ignore_error;
+    this->search_line[3][1] = search_center_h - search_vision_h / 2 - ignore_error;
+    // 鍒ゅ畾鏄惁瀛樺湪
+    if (t_search && t_search->joinable())
+    {
+        this->stop_search_ = true;
+        t_search->join();
+    }
+    this->stop_search_ = false;
+    t_search = new std::thread(&GimbalBase::_search, this, is_roll);
+    this->move_state = SEARCHING;
+    return t_search->joinable();
+}
+
+void GimbalBase::_search(bool is_roll)
+{
+    float t = 0;
+    uint8_t rpy_ctl[3]{2, 2, 2};
+    double rpy_val[3]{0, 0, 0};
+    while (true)
+    {
+        if (this->stop_search_)
+        {
+            return;
+        }
+        for (int i = 0; i < 4 && !this->stop_search_; ++i)
+        {
+            if (is_roll)
+            {
+                rpy_val[0] = this->search_line[i][1];
+                rpy_val[1] = this->search_line[i][0];
+                rpy_val[2] = 0;
+            }
+            else
+            {
+                rpy_val[0] = 0;
+                rpy_val[1] = this->search_line[i][0];
+                rpy_val[2] = this->search_line[i][1];
+            }
+
+            // 绛夊緟鍒拌揪鎸囧畾瑙掑害
+            while (true)
+            {
+                if (this->stop_search_)
+                {
+                    return;
+                }
+                this->ctl_gimbal(rpy_ctl, rpy_val, false);
+                std::cout << "\033[33m"
+                          << "Wait for go to the point"
+                          << "\033[30m"
+                          << "\n"
+                          << "target_roll: " << rpy_val[0] << " "
+                          << "current_roll: " << this->control_feedback_truth[0] << "\n"
+                          << "target_pitch: " << rpy_val[1] << " "
+                          << "current_pitch: " << this->control_feedback_truth[1] << "\n"
+                          << "target_yaw: " << rpy_val[2] << " "
+                          << ":current_yaw: " << this->control_feedback_truth[2] << "\n"
+                          << std::endl;
+                using namespace std::chrono_literals;
+                std::this_thread::sleep_for(10ms);
+                if (is_roll)
+                {
+                    if (std::fabs(rpy_val[0] - this->control_feedback_truth[0]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3)
+                        break;
+                }
+                else
+                {
+                    if (std::fabs(rpy_val[2] - this->control_feedback_truth[2]) < 3 && std::fabs(rpy_val[1] - this->control_feedback_truth[1]) < 3)
+                        break;
+                }
+            }
+        }
+    }
+}
+
+void GimbalBase::tracking(float xy_info[2], int ctl_roll, float kp, float ki, float kd, float ignore_error)
+{
+    // std::cout << "PID: " << kp << " " << ki << " " << kd << std::endl;
+    static std::chrono::system_clock::time_point paste_track_ts = std::chrono::system_clock::now();
+    static float i_x = 0, i_y = 0;
+
+    std::chrono::system_clock::time_point now_ts = std::chrono::system_clock::now();
+    // TODO: 灏卞緢绂昏氨锛屾湁鏃跺�欎細dt鍊间細鍙樻垚0.0001鍑狅紝鍏堜娇鐢ㄦ寚鏁板钩鍧囬《鐫�
+    std::chrono::duration<double> dt = now_ts - paste_track_ts;
+    static std::chrono::duration<double> filter_dt = dt;
+    if (dt.count() > 0.01)
+        filter_dt = 0.5 * filter_dt + 0.5 * dt;
+
+    float x = xy_info[0];
+    float y = xy_info[1];
+    static float paste_xy[2]{x, y};
+
+    float dt_error_x = x - paste_xy[0];
+    float dt_error_y = y - paste_xy[1];
+    static float dt_error[2]{dt_error_x, dt_error_y};
+
+    // 閲嶇疆
+    if (std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) > 50)
+    {
+        paste_xy[0] = x;
+        paste_xy[1] = y;
+        dt_error_x = 0;
+        dt_error_y = 0;
+        dt_error[0] = 0;
+        dt_error[1] = 0;
+        i_x = 0;
+        i_y = 0;
+        // std::cout << ignore_error <<" :閲嶇疆鍘嗗彶鏁版嵁: " << std::abs(std::sqrt(x * x + y * y) - std::sqrt(paste_xy[0] * paste_xy[0] + paste_xy[1] * paste_xy[1])) << std::endl;
+    }
+
+    float vel_x = (dt_error_x - dt_error[0]) / filter_dt.count();
+    float vel_y = (dt_error_y - dt_error[1]) / filter_dt.count();
+    i_x += dt_error_x * filter_dt.count();
+    i_y += dt_error_y * filter_dt.count();
+
+    paste_track_ts = now_ts;
+    paste_xy[0] = x;
+    paste_xy[1] = y;
+    dt_error[0] = dt_error_x;
+    dt_error[1] = dt_error_y;
+    // std::cout << "vel_x: " << vel_x << " vel_y: " << vel_y << " dt: " << dt.count() << std::endl;
+    // kd = 0;
+
+    // TODO 娌℃湁鎻愪緵vel_x, i_x鏃惰嚜宸辫绠�
+    // std::cout << "鍍忕礌璇樊: " << x << " " << y << std::endl;
+    // ignore_error = ignore_error * (get_zoom_val() / max_zoom);
+    x = std::abs(x) > std::fmax(ignore_error, 10) ? (x > 0 ? 1 : -1) * fmax(std::abs(x) * kp, 1) : 0;
+    y = std::abs(y) > std::fmax(ignore_error, 10) ? (y > 0 ? 1 : -1) * fmax(std::abs(y) * kp, 1) : 0;
+    // std::cout << "鍍忕礌璇樊: " << x << " " << y << std::endl;
+
+    double mix_control = (x == 0 || std::abs(x) == 1) ? x : x + ki * i_x + kd * vel_x;
+    double pitch_control = (y == 0 || std::abs(y) == 1) ? y : y + ki * i_y + kd * vel_y;
+
+    // std::cout << "鏈�缁堥�熷害: " << pitch_control << " " << mix_control << std::endl;
+    uint8_t ctl[3];
+    double vel[3];
+    switch (ctl_roll)
+    {
+    case 0: // yaw + pitch
+        ctl[0] = 2;
+        ctl[1] = 1;
+        ctl[2] = 1;
+        vel[0] = 0;
+        vel[1] = pitch_control;
+        vel[2] = mix_control;
+        break;
+    case 1: // roll + pitch
+        ctl[0] = 1;
+        ctl[1] = 1;
+        ctl[2] = 2;
+        vel[0] = -mix_control;
+        vel[1] = pitch_control;
+        vel[2] = 0;
+        break;
+    case 2: // 娣峰悎鎺у埗
+        /*
+        "imu_roll:        ret[0]
+        "imu_pitch:       ret[1]
+        "imu_yaw:         ret[2]
+        "target_roll:     ret[3]
+        "target_pitch:    ret[4]
+        "target_yaw:      ret[5]
+        "rel_roll:        ret[6]
+        "rel_pitch:       ret[7]
+        "rel_yaw:         ret[8]
+        "imu_roll_vel:    ret[9]
+        "imu_pitch_vel:   ret[10]
+        "imu_yaw_vel:     ret[11]
+        "rel_roll_vel:    ret[12]
+        "rel_pitch_vel:   ret[13]
+        "rel_yaw_vel:     ret[14]
+        */
+        // static bool is_switch = true;
+        // if (this->gimbal_state[7] < 70) // pitch + roll
+        // {
+        //     if (is_switch == true)
+        //     {
+        //         is_switch = false;
+        //     }
+        //     else
+        //     {
+        //     }
+        //     /*
+        //     1. roll = (if yaw > 0 ? -1 : 1 ) * (90 - abs(yaw))
+        //     2. yaw = 0
+        //     */
+        // }
+        // else // pitch + yaw
+        // {
+        //     if (is_switch == false)
+        //     {
+        //         is_switch = true;
+        //     }
+        //     else
+        //     {
+        //     }
+        //     /*
+        //     1. yaw = (if roll > 0 ? -1 : 1 ) * (90 - abs(roll))
+        //     2. roll = 0
+        //     */
+        // }
+        break;
+    }
+    this->ctl_gimbal(ctl, vel);
+    this->move_state = TRACKING;
+}
+
+float GimbalBase::get_dt()
+{
+    std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = past_clock - now;
+    past_clock = now;
+    return elapsed_seconds.count();
+};
+
+std::ostream &operator<<(std::ostream &output, GimbalBase *gimbal)
+{
+    while (true)
+    {
+        float ret[12];
+        // std::memset(ret, 0, 15);
+        if (!gimbal->get_info(ret))
+            continue;
+        std::cout << "----------------------gimbal-now-status-------------------------" << std::endl;
+        std::cout << "imu_roll:       " << std::setprecision(6) << ret[0] << std::endl;
+        std::cout << "imu_pitch:      " << std::setprecision(6) << ret[1] << std::endl;
+        std::cout << "imu_yaw:        " << std::setprecision(6) << ret[2] << std::endl;
+        std::cout << "target_roll:    " << std::setprecision(6) << ret[3] << std::endl;
+        std::cout << "target_pitch:   " << std::setprecision(6) << ret[4] << std::endl;
+        std::cout << "target_yaw:     " << std::setprecision(6) << ret[5] << std::endl;
+        std::cout << "rel_roll:       " << std::setprecision(6) << ret[6] << std::endl;
+        std::cout << "rel_pitch:      " << std::setprecision(6) << ret[7] << std::endl;
+        std::cout << "rel_yaw:        " << std::setprecision(6) << ret[8] << std::endl;
+        std::cout << "imu_roll_vel:   " << std::setprecision(6) << ret[9] << std::endl;
+        std::cout << "imu_pitch_vel:  " << std::setprecision(6) << ret[10] << std::endl;
+        std::cout << "imu_yaw_vel:    " << std::setprecision(6) << ret[11] << std::endl;
+        break;
+    }
+    return output;
+};
+
+cv::VideoCapture &GimbalBase::operator>>(cv::Mat &mat)
+{
+    cap >> frame;
+    mat = frame;
+    if (outvideo.isOpened())
+    {
+        outvideo << frame.clone();
+    }
+    return cap;
+};
+
+bool GimbalBase::start_record()
+{
+    if (!outvideo.isOpened())
+    {
+        std::string video_path = this->save_path + time_string_now() + ".mp4";
+        std::string pipeline = "appsrc ! autovideoconvert ! omxh265enc ! matroskamux ! filesink location=" + video_path;
+        outvideo.open(pipeline, CV_FOURCC('M', 'P', '4', 'V'), 25.0, cv::Size(this->camera_width, this->camera_hight));
+        this->is_recording = true;
+    }
+    return outvideo.isOpened();
+}
+void GimbalBase::finish_record()
+{
+    this->is_recording = false;
+    outvideo.release();
+}
+
+bool GimbalBase::get_picture(std::string name)
+{
+    std::string img_path;
+    if (name.empty())
+    {
+        img_path = this->save_path + time_string_now() + ".png";
+    }
+    else
+    {
+        img_path = this->save_path + name + ".png";
+    }
+    return cv::imwrite(img_path.c_str(), frame);
+}
+inline std::string GimbalBase::time_string_now()
+{
+    time_t t = std::time(NULL);
+    tm *local = localtime(&t);
+    char buf[64];
+    std::strftime(buf, 64, "%Y-%m-%d_%H:%M:%S", local);
+    return buf;
+}
+#endif
diff --git a/src/gimbal_server.cpp b/src/gimbal_server.cpp
new file mode 100644
index 0000000..39c1fde
--- /dev/null
+++ b/src/gimbal_server.cpp
@@ -0,0 +1,407 @@
+#include <ros/ros.h>
+#include <string>
+#include <thread>
+#include <std_msgs/String.h>
+#include <std_srvs/SetBool.h>
+#include "gimbal.h"
+#include "ViewLink.h"
+#include "amov_gimbal.h"
+#include "prometheus_gimbal_control/GimbalControl.h"
+#include "prometheus_gimbal_control/GimbalState.h"
+#include "prometheus_gimbal_control/VisionDiff.h"
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+// amovGimbal::gimbal *gimbal = nullptr;
+GimbalBase *g_gimbal = nullptr;
+
+prometheus_gimbal_control::GimbalState g_pub_info;
+bool g_stop_track = false;
+
+void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl)
+{
+    switch (ctl->rpyMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::manual:
+        uint8_t rpy_ctl[3];
+        double rpy_vel[3];
+        rpy_ctl[0] = ctl->roll;
+        rpy_ctl[1] = ctl->yaw;
+        rpy_ctl[2] = ctl->pitch;
+        rpy_vel[0] = ctl->rValue;
+        rpy_vel[1] = ctl->pValue;
+        rpy_vel[2] = ctl->yValue;
+
+
+
+        g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel);
+        
+
+        ROS_INFO_STREAM("Manual "
+                        << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2]
+                        << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]);
+
+        break;
+    case prometheus_gimbal_control::GimbalControl::home:
+        g_gimbal->home();
+        ROS_INFO_STREAM("Back Home");
+        break;
+    case prometheus_gimbal_control::GimbalControl::hold:
+        g_gimbal->hold();
+        ROS_INFO_STREAM("Hold");
+        break;
+    case prometheus_gimbal_control::GimbalControl::fellow:
+        g_gimbal->fellow();
+        ROS_INFO_STREAM("Fellow");
+        break;
+    }
+    switch (ctl->focusMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::focusIn:
+        g_gimbal->focus_in();
+        ROS_INFO_STREAM("Focus In");
+        break;
+    case prometheus_gimbal_control::GimbalControl::focusOut:
+        g_gimbal->focus_out();
+        ROS_INFO_STREAM("Focus Out");
+        break;
+    case prometheus_gimbal_control::GimbalControl::focusStop:
+        g_gimbal->focus_stop();
+        ROS_INFO_STREAM("Focus Stop");
+        break;
+    }
+    switch (ctl->zoomMode)
+    {
+    case prometheus_gimbal_control::GimbalControl::zoomIn:
+        g_gimbal->zoom_in();
+        ROS_INFO_STREAM("Zoom In");
+        break;
+
+    case prometheus_gimbal_control::GimbalControl::zoomOut:
+        g_gimbal->zoom_out();
+        ROS_INFO_STREAM("Zoom Out");
+        break;
+    case prometheus_gimbal_control::GimbalControl::zoomStop:
+        g_gimbal->zoom_stop();
+        ROS_INFO_STREAM("Zoom Stop");
+        break;
+    }
+};
+
+void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &cent)
+{
+    if (g_stop_track)
+        return;
+    float tmp_cent[2]{0, 0};
+    tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2;
+    tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2;
+    int roll_mode = cent->ctlMode;
+    float kp = cent->kp;
+    float ki = cent->ki;
+    float kd = cent->kd;
+
+    static bool flag_zoom = false;
+    static bool flag_hold = false;
+
+    if (cent->detect != 0)
+    {
+        int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1;
+        float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom;
+        g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom);
+        // std::cout << "step 3 : " << std::endl;
+        if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError)
+        {
+            g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize);
+            flag_zoom = true;
+        }
+        // std::cout << "step 4 : " << std::endl;
+        flag_hold = true;
+    }
+    else
+    {
+        if (cent->currSize && flag_zoom)
+        {
+            g_gimbal->zoom_stop();
+            flag_zoom = false;
+        }
+        if (flag_hold)
+        {
+            g_gimbal->hold();
+            flag_hold = false;
+        }
+    }
+};
+
+void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info)
+{
+    static bool flag = false;
+    int now_flag = 0;
+    int zoom_val = std::fmax(g_gimbal->zoom_val, 1);
+    if (info->detect != 0 && (std::abs(g_pub_info.imuAngleVel[0]) + std::abs(g_pub_info.imuAngleVel[1]) + std::abs(g_pub_info.imuAngleVel[2])) < 3)
+    {
+        g_gimbal->auto_zoom(info->currSize, zoom_val, info->maxSize, info->minSize);
+        flag = true;
+    }
+    else
+    {
+        if (flag)
+        {
+            g_gimbal->zoom_stop();
+            flag = false;
+        }
+    }
+}
+
+bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req)
+{
+    if (ref.data)
+    {
+        g_gimbal->start_record();
+        req.success = g_gimbal->outvideo.isOpened();
+        ROS_INFO_STREAM("Start Video Record ! ");
+    }
+    else
+    {
+        if (!g_gimbal->outvideo.isOpened())
+        {
+            req.success = false;
+            req.message = "video record not is started ! ";
+            ROS_WARN_STREAM("video record not is started ! ");
+        }
+        g_gimbal->finish_record();
+        if (g_gimbal->outvideo.isOpened())
+        {
+            req.success = false;
+            req.message = "finish video fail ! ";
+            ROS_WARN_STREAM("finish video fail ! ");
+        }
+        else
+        {
+            req.success = true;
+            ROS_INFO_STREAM("Finish Video Record ! ");
+        }
+    }
+    return true;
+}
+
+void picCallback(const std_msgs::String::ConstPtr &info)
+{
+    ROS_INFO("Get Picture ");
+    g_gimbal->get_picture();
+}
+
+void callAll(image_transport::Publisher *image_pub)
+{
+    cv::Mat frame;
+    while (true)
+    {
+        (*g_gimbal) >> frame;
+        if (!frame.empty())
+        {
+            image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
+        }
+        std::this_thread::sleep_for(10ms);
+    }
+}
+
+bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    resp.success = g_gimbal->search(req.data);
+    return true;
+}
+
+// 鍒囨崲鍙嶉妯″紡锛岃嚜鍔紵浜ゆ崲
+bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity = req.data;
+    resp.success = true;
+    resp.message = static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed";
+    return true;
+}
+
+// 灞忚斀tracking鎺у埗
+bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
+{
+    g_stop_track = req.data;
+    resp.success = true;
+    return true;
+}
+
+int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam)
+{
+    if (VLK_DEV_STATUS_TYPE_MODEL == iType)
+    {
+        VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer;
+        std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl;
+    }
+    else if (VLK_DEV_STATUS_TYPE_CONFIG == iType)
+    {
+        VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer;
+        std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl;
+    }
+    else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType)
+    {
+        /*
+         * once device is connected, telemetry information will keep updating,
+         * in order to avoid disturbing user input, comment out printing telemetry information
+         */
+        // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer;
+        // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl;
+    }
+    else
+    {
+        std::cout << "error: unknown status type: " << iType << std::endl;
+    }
+
+
+    return 0;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "gimbal");
+    ros::NodeHandle n("~");
+    image_transport::ImageTransport it(n);
+    image_transport::Publisher image_pub;
+    cv::Mat frame;
+    sensor_msgs::ImagePtr img;
+    // rpy鎺у埗, 鍙樼劍, 鏀惧ぇ
+    image_pub = it.advertise("image_raw", 1);
+    ros::Subscriber sub_ctl = n.subscribe<prometheus_gimbal_control::GimbalControl>("control", 10, ctlCallback);
+    ros::Subscriber sub_track = n.subscribe<prometheus_gimbal_control::VisionDiff>("track", 10, trackCallback);
+    ros::Subscriber save_pic = n.subscribe<std_msgs::String>("get_pic", 100, picCallback);
+    ros::Publisher pub_rpy = n.advertise<prometheus_gimbal_control::GimbalState>("state", 10);
+    ros::ServiceServer srv_search = n.advertiseService("search", search);
+    ros::ServiceServer srv_video = n.advertiseService("record_video", videoRecord);
+    ros::ServiceServer srv_track = n.advertiseService("stop_track", stopTrack);
+
+    ros::Rate rate(20);
+
+    int camera_width, camera_height, uav_id;
+    std::string camera_id;
+    std::string tty_url, save_path, gimbal_type;
+    n.param<std::string>("camera_id", camera_id, "0");
+    n.param<int>("uav_id", uav_id, 0); // 鏃犱汉鏈虹殑ID
+    n.param<std::string>("tty_url", tty_url, "/dev/ttyUSB0");
+    n.param<int>("camera_width", camera_width, 1920);
+    n.param<int>("camera_height", camera_height, 1080);
+    n.param<std::string>("save_path", save_path, "gimbal_video_data");
+    n.param<std::string>("gimbal_type", gimbal_type, "at10");
+
+    if (gimbal_type == "q10f")
+    {
+        g_gimbal = new GimbalQ10f(save_path);
+    }
+    else if (gimbal_type == "g1")
+    {
+        g_gimbal = new AmovG1(save_path);
+    }
+    else if (gimbal_type == "at10")
+    {
+        g_gimbal = new GimbalAT10(save_path);
+    }
+    else
+    {
+        ROS_ERROR_STREAM("NOT SUPPORT " << gimbal_type);
+        return -1;
+    }
+
+    try
+    {
+        g_gimbal->open_tty(tty_url);
+    }
+    catch (...)
+    {
+        ROS_ERROR_STREAM("Unable to open Serial Port ! ");
+        getchar();
+        return -1;
+    }
+
+    try
+    {
+        // 浠�60甯х巼鎵撳紑锛屼粎瀵笰MOV G1鏈夋晥
+        g_gimbal->open_camera(camera_id, camera_width, camera_height, 60);
+    }
+    catch (...)
+    {
+        ROS_ERROR_STREAM("Unable to open Camera ! ");
+        getchar();
+        return -1;
+    }
+
+    if (g_gimbal->device_name == "q10f" || g_gimbal->device_name == "at10")
+    {
+        static ros::ServiceServer srv_track_mode = n.advertiseService("track_mode", switchTrack);
+        static ros::Subscriber sub_currSize = n.subscribe<prometheus_gimbal_control::VisionDiff>("currSize", 10, currSizeCallback);
+        static_cast<GimbalQ10f *>(g_gimbal)->stopAuto();
+        static_cast<GimbalAT10 *>(g_gimbal)->stopAuto();
+        // initialize sdk
+        int iRet = VLK_Init();
+        if (VLK_ERROR_NO_ERROR != iRet)
+        {
+        std::cout << "VLK_Init failed, error: " << iRet << std::endl;
+        return -1;
+        }
+        // register device status callback
+        VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL);
+        // ROS_INFO_STREAM("AT10 !");
+        // 5绉掑悗鍋滄 zoom_out()
+        static ros::Timer stop_zoom = n.createTimer(
+            ros::Duration(5.0), [&](const ros::TimerEvent &)
+            { g_gimbal->zoom_out(); },
+            true);
+    }
+
+    g_gimbal->home();
+    g_gimbal->zoom_out();
+    ROS_INFO_STREAM("Program Started ! ");
+    std::thread spin(callAll, &image_pub);
+    if(g_gimbal->device_name == "AT10")
+    {
+        ROS_INFO_STREAM("at10 !");
+    }else if(g_gimbal->device_name == "q10f")
+    {
+        ROS_INFO_STREAM("q10f !");
+    }else if(g_gimbal->device_name == "g1"){
+        ROS_INFO_STREAM("g1 !");
+        // print sdk version
+        std::cout << "ViewLink SDK version: " << GetSDKVersion() << std::endl;
+    }else {
+        ROS_INFO_STREAM("unknow device!!!");
+        std::cout << "device name: " << g_gimbal->device_name << std::endl;
+        std::cout << "device type: " << gimbal_type << std::endl;
+    }
+
+
+    g_pub_info.Id = uav_id;
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        float info[12];
+        g_gimbal->get_info(info);
+        g_pub_info.mode = g_gimbal->move_state;
+        g_pub_info.feedbackMode = g_gimbal->feedback_state;
+        g_pub_info.isRecording = g_gimbal->is_recording;
+        g_pub_info.zoomState = g_gimbal->zoom_state;
+        g_pub_info.zoomVal = g_gimbal->fzoom_val;
+        g_pub_info.imuAngle[0] = info[0];
+        g_pub_info.imuAngle[1] = info[1];
+        g_pub_info.imuAngle[2] = info[2];
+        g_pub_info.rotorAngleTarget[0] = info[3];
+        g_pub_info.rotorAngleTarget[1] = info[4];
+        g_pub_info.rotorAngleTarget[2] = info[5];
+        g_pub_info.rotorAngle[0] = info[6];
+        g_pub_info.rotorAngle[1] = info[7];
+        g_pub_info.rotorAngle[2] = info[8];
+        g_pub_info.imuAngleVel[0] = info[9];
+        g_pub_info.imuAngleVel[1] = info[10];
+        g_pub_info.imuAngleVel[2] = info[11];
+        pub_rpy.publish(g_pub_info);
+        rate.sleep();
+        // std::cout << "run ........" << std::endl;
+    }
+    delete g_gimbal;
+}
diff --git a/src/serial.cpp b/src/serial.cpp
new file mode 100644
index 0000000..a1dae49
--- /dev/null
+++ b/src/serial.cpp
@@ -0,0 +1,293 @@
+#include <algorithm>
+
+#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
+#include <alloca.h>
+#endif
+
+#include "serial/serial.h"
+
+#include "serial/impl/unix.h"
+
+using std::invalid_argument;
+using std::min;
+using std::numeric_limits;
+using std::size_t;
+using std::string;
+using std::vector;
+
+using serial::bytesize_t;
+using serial::flowcontrol_t;
+using serial::IOException;
+using serial::parity_t;
+using serial::Serial;
+using serial::SerialException;
+using serial::stopbits_t;
+
+class Serial::ScopedReadLock {
+public:
+  ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->readLock();
+  }
+  ~ScopedReadLock() { this->pimpl_->readUnlock(); }
+
+private:
+  // Disable copy constructors
+  ScopedReadLock(const ScopedReadLock &);
+  const ScopedReadLock &operator=(ScopedReadLock);
+
+  SerialImpl *pimpl_;
+};
+
+class Serial::ScopedWriteLock {
+public:
+  ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) {
+    this->pimpl_->writeLock();
+  }
+  ~ScopedWriteLock() { this->pimpl_->writeUnlock(); }
+
+private:
+  // Disable copy constructors there 
+  ScopedWriteLock(const ScopedWriteLock &);
+  const ScopedWriteLock &operator=(ScopedWriteLock);
+  SerialImpl *pimpl_;
+};
+
+Serial::Serial(const string &port, uint32_t baudrate, serial::Timeout timeout,
+               bytesize_t bytesize, parity_t parity, stopbits_t stopbits,
+               flowcontrol_t flowcontrol)
+    : pimpl_(new SerialImpl(port, baudrate, bytesize, parity, stopbits,
+                            flowcontrol)) {
+  pimpl_->setTimeout(timeout);
+}
+
+Serial::~Serial() { delete pimpl_; }
+
+void Serial::open() { pimpl_->open(); }
+
+void Serial::close() { pimpl_->close(); }
+
+bool Serial::isOpen() const { return pimpl_->isOpen(); }
+
+size_t Serial::available() { return pimpl_->available(); }
+
+bool Serial::waitReadable() {
+  serial::Timeout timeout(pimpl_->getTimeout());
+  return pimpl_->waitReadable(timeout.read_timeout_constant);
+}
+
+void Serial::waitByteTimes(size_t count) { pimpl_->waitByteTimes(count); }
+
+size_t Serial::read_(uint8_t *buffer, size_t size) {
+  return this->pimpl_->read(buffer, size);
+}
+
+size_t Serial::read(uint8_t *buffer, size_t size) {
+  ScopedReadLock lock(this->pimpl_);
+  return this->pimpl_->read(buffer, size);
+}
+
+size_t Serial::read(std::vector<uint8_t> &buffer, size_t size) {
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+
+  try {
+    bytes_read = this->pimpl_->read(buffer_, size);
+  } catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+
+  buffer.insert(buffer.end(), buffer_, buffer_ + bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+size_t Serial::read(std::string &buffer, size_t size) {
+  ScopedReadLock lock(this->pimpl_);
+  uint8_t *buffer_ = new uint8_t[size];
+  size_t bytes_read = 0;
+  try {
+    bytes_read = this->pimpl_->read(buffer_, size);
+  } catch (const std::exception &e) {
+    delete[] buffer_;
+    throw;
+  }
+  buffer.append(reinterpret_cast<const char *>(buffer_), bytes_read);
+  delete[] buffer_;
+  return bytes_read;
+}
+
+string Serial::read(size_t size) {
+  std::string buffer;
+  this->read(buffer, size);
+  return buffer;
+}
+
+size_t Serial::readline(string &buffer, size_t size, string eol) {
+  ScopedReadLock lock(this->pimpl_);
+  size_t eol_len = eol.length();
+  uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
+  size_t read_so_far = 0;
+  while (true) {
+    size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      break; // Timeout occured on reading 1 byte
+    }
+    if (read_so_far < eol_len)
+      continue;
+    if (string(reinterpret_cast<const char *>(buffer_ + read_so_far - eol_len),
+               eol_len) == eol) {
+      break; // EOL found
+    }
+    if (read_so_far == size) {
+      break; // Reached the maximum read length
+    }
+  }
+  buffer.append(reinterpret_cast<const char *>(buffer_), read_so_far);
+  return read_so_far;
+}
+
+string Serial::readline(size_t size, string eol) {
+  std::string buffer;
+  this->readline(buffer, size, eol);
+  return buffer;
+}
+
+vector<string> Serial::readlines(size_t size, string eol) {
+  ScopedReadLock lock(this->pimpl_);
+  std::vector<std::string> lines;
+  size_t eol_len = eol.length();
+  uint8_t *buffer_ = static_cast<uint8_t *>(alloca(size * sizeof(uint8_t)));
+  size_t read_so_far = 0;
+  size_t start_of_line = 0;
+  while (read_so_far < size) {
+    size_t bytes_read = this->read_(buffer_ + read_so_far, 1);
+    read_so_far += bytes_read;
+    if (bytes_read == 0) {
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+            string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                   read_so_far - start_of_line));
+      }
+      break; // Timeout occured on reading 1 byte
+    }
+    if (read_so_far < eol_len)
+      continue;
+    if (string(reinterpret_cast<const char *>(buffer_ + read_so_far - eol_len),
+               eol_len) == eol) {
+      // EOL found
+      lines.push_back(
+          string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                 read_so_far - start_of_line));
+      start_of_line = read_so_far;
+    }
+    if (read_so_far == size) {
+      if (start_of_line != read_so_far) {
+        lines.push_back(
+            string(reinterpret_cast<const char *>(buffer_ + start_of_line),
+                   read_so_far - start_of_line));
+      }
+      break; // Reached the maximum read length
+    }
+  }
+  return lines;
+}
+
+size_t Serial::write(const string &data) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(reinterpret_cast<const uint8_t *>(data.c_str()),
+                      data.length());
+}
+
+size_t Serial::write(const std::vector<uint8_t> &data) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(&data[0], data.size());
+}
+
+size_t Serial::write(const uint8_t *data, size_t size) {
+  ScopedWriteLock lock(this->pimpl_);
+  return this->write_(data, size);
+}
+
+size_t Serial::write_(const uint8_t *data, size_t length) {
+  return pimpl_->write(data, length);
+}
+
+void Serial::setPort(const string &port) {
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  bool was_open = pimpl_->isOpen();
+  if (was_open)
+    close();
+  pimpl_->setPort(port);
+  if (was_open)
+    open();
+}
+
+string Serial::getPort() const { return pimpl_->getPort(); }
+
+void Serial::setTimeout(serial::Timeout &timeout) {
+  pimpl_->setTimeout(timeout);
+}
+
+serial::Timeout Serial::getTimeout() const { return pimpl_->getTimeout(); }
+
+void Serial::setBaudrate(uint32_t baudrate) { pimpl_->setBaudrate(baudrate); }
+
+uint32_t Serial::getBaudrate() const { return uint32_t(pimpl_->getBaudrate()); }
+
+void Serial::setBytesize(bytesize_t bytesize) { pimpl_->setBytesize(bytesize); }
+
+bytesize_t Serial::getBytesize() const { return pimpl_->getBytesize(); }
+
+void Serial::setParity(parity_t parity) { pimpl_->setParity(parity); }
+
+parity_t Serial::getParity() const { return pimpl_->getParity(); }
+
+void Serial::setStopbits(stopbits_t stopbits) { pimpl_->setStopbits(stopbits); }
+
+stopbits_t Serial::getStopbits() const { return pimpl_->getStopbits(); }
+
+void Serial::setFlowcontrol(flowcontrol_t flowcontrol) {
+  pimpl_->setFlowcontrol(flowcontrol);
+}
+
+flowcontrol_t Serial::getFlowcontrol() const {
+  return pimpl_->getFlowcontrol();
+}
+
+void Serial::flush() {
+  ScopedReadLock rlock(this->pimpl_);
+  ScopedWriteLock wlock(this->pimpl_);
+  pimpl_->flush();
+}
+
+void Serial::flushInput() {
+  ScopedReadLock lock(this->pimpl_);
+  pimpl_->flushInput();
+}
+
+void Serial::flushOutput() {
+  ScopedWriteLock lock(this->pimpl_);
+  pimpl_->flushOutput();
+}
+
+void Serial::sendBreak(int duration) { pimpl_->sendBreak(duration); }
+
+void Serial::setBreak(bool level) { pimpl_->setBreak(level); }
+
+void Serial::setRTS(bool level) { pimpl_->setRTS(level); }
+
+void Serial::setDTR(bool level) { pimpl_->setDTR(level); }
+
+bool Serial::waitForChange() { return pimpl_->waitForChange(); }
+
+bool Serial::getCTS() { return pimpl_->getCTS(); }
+
+bool Serial::getDSR() { return pimpl_->getDSR(); }
+
+bool Serial::getRI() { return pimpl_->getRI(); }
+
+bool Serial::getCD() { return pimpl_->getCD(); }