From 0459704fdebf21d49061297cc2d01ef14a516a29 Mon Sep 17 00:00:00 2001 From: laptoy <2107884590@qq.com> Date: Fri, 7 Jun 2024 21:14:57 +0800 Subject: [PATCH] load --- src/GPS/CMakeLists.txt | 26 - src/GPS/GPSManager.h | 42 - src/GPS/GPSPositionMessage.h | 26 - src/GPS/GPSProvider.h | 84 -- src/GPS/RTCM/RTCMMavlink.cc | 76 -- src/GPS/RTCM/RTCMMavlink.h | 33 - src/GPS/definitions.h | 63 -- src/GPS/satellite_info.h | 24 - src/GPS/vehicle_gps_position.h | 39 - src/{GPS/GPSManager.cc => GPSManager.cpp} | 0 src/{GPS/GPSProvider.cc => GPSProvider.cpp} | 0 src/JsonHelper.cpp | 520 +++++++++++++ src/ViewLink.h | 822 ++++++++++++++++++++ src/gimbal_base.hpp | 454 +++++++++++ src/gimbal_server.cpp | 407 ++++++++++ src/serial.cpp | 293 +++++++ 16 files changed, 2496 insertions(+), 413 deletions(-) delete mode 100644 src/GPS/CMakeLists.txt delete mode 100644 src/GPS/GPSManager.h delete mode 100644 src/GPS/GPSPositionMessage.h delete mode 100644 src/GPS/GPSProvider.h delete mode 100644 src/GPS/RTCM/RTCMMavlink.cc delete mode 100644 src/GPS/RTCM/RTCMMavlink.h delete mode 100644 src/GPS/definitions.h delete mode 100644 src/GPS/satellite_info.h delete mode 100644 src/GPS/vehicle_gps_position.h rename src/{GPS/GPSManager.cc => GPSManager.cpp} (100%) rename src/{GPS/GPSProvider.cc => GPSProvider.cpp} (100%) create mode 100644 src/JsonHelper.cpp create mode 100644 src/ViewLink.h create mode 100644 src/gimbal_base.hpp create mode 100644 src/gimbal_server.cpp create mode 100644 src/serial.cpp 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 - -#include -#include - -/** - ** 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 - -/** - ** 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 -#include -#include -#include - -#include - -#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 - -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(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 -#include - -#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 - */ - -#pragma once - -#include - -#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 - -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 -/** - * 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 -#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 - -/* - * 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 - -/* - * 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 +#include +#include +#include +#include +#include +#include + +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& types, QString& errorString) +{ + for (int i=0; i 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 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& rgPoints, + QString& errorString) +{ + QVariantList rgVarPoints; + + if (!loadGeoCoordinateArray(jsonValue, altitudeRequired, rgVarPoints, errorString)) { + return false; + } + + rgPoints.clear(); + for (int i=0; i()); + } + + 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(), writeAltitude, jsonPoint); + rgJsonPoints.append(jsonPoint); + } + + jsonValue = rgJsonPoints; +} + +void JsonHelper::saveGeoCoordinateArray(const QList& rgPoints, + bool writeAltitude, + QJsonValue& jsonValue) +{ + QVariantList rgVarPoints; + + for (int i=0; i& keyInfo, QString& errorString) +{ + QStringList keyList; + QList typeList; + + for (int i=0; i(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::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 +#include +#include +#include +// #include +#include + +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 dt = now_ts - paste_track_ts; + static std::chrono::duration 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 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 +#include +#include +#include +#include +#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 +#include +#include +#include +#include + +// 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 ¢) +{ + 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(g_gimbal)->isGetRealVelocity = req.data; + resp.success = true; + resp.message = static_cast(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("control", 10, ctlCallback); + ros::Subscriber sub_track = n.subscribe("track", 10, trackCallback); + ros::Subscriber save_pic = n.subscribe("get_pic", 100, picCallback); + ros::Publisher pub_rpy = n.advertise("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("camera_id", camera_id, "0"); + n.param("uav_id", uav_id, 0); // 鏃犱汉鏈虹殑ID + n.param("tty_url", tty_url, "/dev/ttyUSB0"); + n.param("camera_width", camera_width, 1920); + n.param("camera_height", camera_height, 1080); + n.param("save_path", save_path, "gimbal_video_data"); + n.param("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("currSize", 10, currSizeCallback); + static_cast(g_gimbal)->stopAuto(); + static_cast(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 + +#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__) +#include +#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 &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(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(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(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(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 Serial::readlines(size_t size, string eol) { + ScopedReadLock lock(this->pimpl_); + std::vector lines; + size_t eol_len = eol.length(); + uint8_t *buffer_ = static_cast(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(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(buffer_ + read_so_far - eol_len), + eol_len) == eol) { + // EOL found + lines.push_back( + string(reinterpret_cast(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(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(data.c_str()), + data.length()); +} + +size_t Serial::write(const std::vector &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(); }