pxsi3jbgc
/
wwcs-1314
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

load

Browse Source
pull/5/head
laptoy 6 months ago
parent 3b83e60352
commit 0459704fde
16 changed files with 2496 additions and 413 deletions
Show Stats Download Patch File Download Diff File

26
src/GPS/CMakeLists.txt
Unescape View File

@ -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})

42
src/GPS/GPSManager.h
Unescape View File

@ -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
};

26
src/GPS/GPSPositionMessage.h
Unescape View File

@ -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);

84
src/GPS/GPSProvider.h
Unescape View File

@ -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;
};

76
src/GPS/RTCM/RTCMMavlink.cc
Unescape View File

@ -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);
}
}
}

33
src/GPS/RTCM/RTCMMavlink.h
Unescape View File

@ -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;
};

63
src/GPS/definitions.h
Unescape View File

@ -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

24
src/GPS/satellite_info.h
Unescape View File

@ -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
};

39
src/GPS/vehicle_gps_position.h
Unescape View File

@ -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;
};

0
src/GPS/GPSManager.cc → src/GPSManager.cpp
Unescape View File

0
src/GPS/GPSProvider.cc → src/GPSProvider.cpp
Unescape View File

520
src/JsonHelper.cpp
Unescape View File

@ -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();
}
}

822
src/ViewLink.h
Unescape View File

@ -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__

454
src/gimbal_base.hpp
Unescape View File

@ -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电机角度，roll,pitch到imu角度
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

407
src/gimbal_server.cpp
Unescape View File

@ -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帧率打开，仅对AMOV 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;
}

293
src/serial.cpp
Unescape View File

@ -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(); }
Write Preview
Loading…
Cancel
Save