Browse Source

Get (and expose) link status

QGC4.4
Gus Grubba 6 years ago
parent
commit
bab3c35af8
  1. 1
      src/Taisync/TaisyncHandler.cc
  2. 3
      src/Taisync/TaisyncHandler.h
  3. 52
      src/Taisync/TaisyncSettings.cc
  4. 20
      src/Taisync/TaisyncSettings.h
  5. 29
      src/comm/TaisyncLink.cc
  6. 5
      src/comm/TaisyncLink.h

1
src/Taisync/TaisyncHandler.cc

@ -60,6 +60,7 @@ TaisyncHandler::_newConnection() @@ -60,6 +60,7 @@ TaisyncHandler::_newConnection()
}
_tcpSocket = _tcpServer->nextPendingConnection();
QObject::connect(_tcpSocket, &QIODevice::readyRead, this, &TaisyncHandler::_readBytes);
emit connected();
}
//-----------------------------------------------------------------------------

3
src/Taisync/TaisyncHandler.h

@ -39,6 +39,9 @@ protected slots: @@ -39,6 +39,9 @@ protected slots:
virtual void _socketDisconnected ();
virtual void _readBytes () = 0;
signals:
void connected ();
protected:
QTcpServer* _tcpServer = nullptr;
QTcpSocket* _tcpSocket = nullptr;

52
src/Taisync/TaisyncSettings.cc

@ -15,6 +15,14 @@ @@ -15,6 +15,14 @@
QGC_LOGGING_CATEGORY(TaisyncSettingsLog, "TaisyncSettingsLog")
static const char* kPostReq =
"POST %1 HTTP/1.1\r\n"
"Content-Type: application/json\r\n"
"Content-Length: %2\r\n\r\n"
"%3";
static const char* kGetReq = "GET %1 HTTP/1.1\r\n\r\n";
//-----------------------------------------------------------------------------
TaisyncSettings::TaisyncSettings(QObject* parent)
: TaisyncHandler(parent)
@ -30,10 +38,54 @@ TaisyncSettings::start() @@ -30,10 +38,54 @@ TaisyncSettings::start()
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings::requestSettings()
{
if(_tcpSocket) {
QString req = QString(kGetReq).arg("/v1/baseband.json");
_tcpSocket->write(req.toUtf8());
return true;
}
return false;
}
//-----------------------------------------------------------------------------
bool
TaisyncSettings::requestFreqScan()
{
if(_tcpSocket) {
QString req = QString(kGetReq).arg("/v1/freqscan.json");
_tcpSocket->write(req.toUtf8());
return true;
}
return false;
}
//-----------------------------------------------------------------------------
void
TaisyncSettings::_readBytes()
{
QByteArray bytesIn = _tcpSocket->read(_tcpSocket->bytesAvailable());
qCDebug(TaisyncSettingsLog) << "Taisync settings data:" << bytesIn.size();
qCDebug(TaisyncSettingsLog) << QString(bytesIn);
if(bytesIn.contains("200 OK")) {
//-- Link Status?
int idx = bytesIn.indexOf('{');
QJsonParseError jsonParseError;
QJsonDocument doc = QJsonDocument::fromJson(bytesIn.mid(idx), &jsonParseError);
if (jsonParseError.error != QJsonParseError::NoError) {
qWarning() << "Unable to parse Taisync response:" << jsonParseError.errorString() << jsonParseError.offset;
return;
}
QJsonObject jObj = doc.object();
//-- Link Status?
if(bytesIn.contains("\"flight\":")) {
_linkConnected = jObj["flight"].toBool(_linkConnected);
_linkVidFormat = jObj["videoformat"].toString(_linkVidFormat);
_downlinkRSSI = jObj["radiorssi"].toInt(_downlinkRSSI);
_uplinkRSSI = jObj["hdrssi"].toInt(_uplinkRSSI);
emit linkChanged();
}
}
}

20
src/Taisync/TaisyncSettings.h

@ -18,10 +18,30 @@ class TaisyncSettings : public TaisyncHandler @@ -18,10 +18,30 @@ class TaisyncSettings : public TaisyncHandler
Q_OBJECT
public:
Q_PROPERTY(bool linkConnected READ linkConnected NOTIFY linkChanged)
Q_PROPERTY(QString linkVidFormat READ linkVidFormat NOTIFY linkChanged)
Q_PROPERTY(int uplinkRSSI READ uplinkRSSI NOTIFY linkChanged)
Q_PROPERTY(int downlinkRSSI READ downlinkRSSI NOTIFY linkChanged)
explicit TaisyncSettings (QObject* parent = nullptr);
void start () override;
bool requestSettings ();
bool requestFreqScan ();
bool linkConnected () { return _linkConnected; }
QString linkVidFormat () { return _linkVidFormat; }
int uplinkRSSI () { return _downlinkRSSI; }
int downlinkRSSI () { return _uplinkRSSI; }
signals:
void linkChanged ();
protected slots:
void _readBytes () override;
private:
bool _linkConnected = false;
QString _linkVidFormat;
int _downlinkRSSI = 0;
int _uplinkRSSI = 0;
};

29
src/comm/TaisyncLink.cc

@ -50,6 +50,7 @@ TaisyncLink::~TaisyncLink() @@ -50,6 +50,7 @@ TaisyncLink::~TaisyncLink()
void
TaisyncLink::run()
{
// Thread
if(_hardwareConnect()) {
exec();
}
@ -96,9 +97,11 @@ TaisyncLink::_readBytes(QByteArray bytes) @@ -96,9 +97,11 @@ TaisyncLink::_readBytes(QByteArray bytes)
void
TaisyncLink::_disconnect()
{
//-- Stop thread
_running = false;
quit();
wait();
//-- Kill Taisync handlers
if (_taiTelemetery) {
_hardwareDisconnect();
emit disconnected();
@ -129,6 +132,7 @@ TaisyncLink::_connect(void) @@ -129,6 +132,7 @@ TaisyncLink::_connect(void)
if(_taiConfig->videoEnabled()) {
//-- Hide video selection as we will be fixed to Taisync video and set the way we need it.
VideoSettings* pVSettings = qgcApp()->toolbox()->settingsManager()->videoSettings();
//-- First save current state
_savedVideoSource = pVSettings->videoSource()->rawValue();
_savedVideoUDP = pVSettings->udpPort()->rawValue();
_savedAR = pVSettings->aspectRatio()->rawValue();
@ -137,6 +141,7 @@ TaisyncLink::_connect(void) @@ -137,6 +141,7 @@ TaisyncLink::_connect(void)
//-- iOS and Android receive raw h.264 and need a different pipeline
qgcApp()->toolbox()->videoManager()->setIsTaisync(true);
#endif
//-- Now set it up the way we need it do be
pVSettings->setVisible(false);
pVSettings->udpPort()->setRawValue(5600);
pVSettings->aspectRatio()->setRawValue(1024.0 / 768.0);
@ -167,6 +172,7 @@ TaisyncLink::_hardwareDisconnect() @@ -167,6 +172,7 @@ TaisyncLink::_hardwareDisconnect()
_taiVideo = nullptr;
}
#endif
_connected = false;
}
//-----------------------------------------------------------------------------
@ -176,9 +182,11 @@ TaisyncLink::_hardwareConnect() @@ -176,9 +182,11 @@ TaisyncLink::_hardwareConnect()
_hardwareDisconnect();
_taiTelemetery = new TaisyncTelemetry(this);
QObject::connect(_taiTelemetery, &TaisyncTelemetry::bytesReady, this, &TaisyncLink::_readBytes);
QObject::connect(_taiTelemetery, &TaisyncTelemetry::connected, this, &TaisyncLink::_telemetryReady);
_taiTelemetery->start();
_taiSettings = new TaisyncSettings(this);
_taiSettings->start();
QObject::connect(_taiSettings, &TaisyncSettings::connected, this, &TaisyncLink::_settingsReady);
#if defined(__ios__) || defined(__android__)
if(_taiConfig->videoEnabled()) {
_taiVideo = new TaisyncVideoReceiver(this);
@ -192,7 +200,7 @@ TaisyncLink::_hardwareConnect() @@ -192,7 +200,7 @@ TaisyncLink::_hardwareConnect()
bool
TaisyncLink::isConnected() const
{
return _taiTelemetery != nullptr;
return _connected;
}
//-----------------------------------------------------------------------------
@ -216,6 +224,25 @@ TaisyncLink::getCurrentOutDataRate() const @@ -216,6 +224,25 @@ TaisyncLink::getCurrentOutDataRate() const
return 0;
}
//-----------------------------------------------------------------------------
void
TaisyncLink::_telemetryReady()
{
qCDebug(TaisyncLog) << "Taisync telemetry ready";
if(!_connected) {
_connected = true;
emit connected();
}
}
//-----------------------------------------------------------------------------
void
TaisyncLink::_settingsReady()
{
qCDebug(TaisyncLog) << "Taisync settings ready";
_taiSettings->requestSettings();
}
//--------------------------------------------------------------------------
//-- TaisyncConfiguration

5
src/comm/TaisyncLink.h

@ -81,6 +81,8 @@ public: @@ -81,6 +81,8 @@ public:
bool isConnected () const override;
QString getName () const override;
TaisyncSettings*taisyncSettings () { return _taiSettings; }
// Extensive statistics for scientific purposes
qint64 getConnectionSpeed () const override;
qint64 getCurrentInDataRate () const;
@ -97,6 +99,8 @@ public: @@ -97,6 +99,8 @@ public:
private slots:
void _writeBytes (const QByteArray data) override;
void _readBytes (QByteArray bytes);
void _telemetryReady ();
void _settingsReady ();
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
@ -121,6 +125,7 @@ private: @@ -121,6 +125,7 @@ private:
TaisyncVideoReceiver* _taiVideo = nullptr;
#endif
bool _savedVideoState = true;
bool _connected = false;
QVariant _savedVideoSource;
QVariant _savedVideoUDP;
QVariant _savedAR;

Loading…
Cancel
Save