DonLakeFlyer
5 years ago
24 changed files with 609 additions and 238 deletions
@ -0,0 +1,68 @@
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ADSBVehicle.h" |
||||
#include "QGCLoggingCategory.h" |
||||
|
||||
#include <QDebug> |
||||
#include <QtMath> |
||||
|
||||
ADSBVehicle::ADSBVehicle(const VehicleInfo_t& vehicleInfo, QObject* parent) |
||||
: QObject (parent) |
||||
, _icaoAddress (vehicleInfo.icaoAddress) |
||||
, _altitude (qQNaN()) |
||||
, _heading (qQNaN()) |
||||
, _alert (false) |
||||
{ |
||||
update(vehicleInfo); |
||||
} |
||||
|
||||
void ADSBVehicle::update(const VehicleInfo_t& vehicleInfo) |
||||
{ |
||||
if (_icaoAddress != vehicleInfo.icaoAddress) { |
||||
qCWarning(ADSBVehicleManagerLog) << "ICAO address mismatch expected:actual" << _icaoAddress << vehicleInfo.icaoAddress; |
||||
return; |
||||
} |
||||
if (vehicleInfo.availableFlags & CallsignAvailable) { |
||||
if (vehicleInfo.callsign != _callsign) { |
||||
_callsign = vehicleInfo.callsign; |
||||
emit callsignChanged(); |
||||
} |
||||
} |
||||
if (vehicleInfo.availableFlags & LocationAvailable) { |
||||
if (_coordinate != vehicleInfo.location) { |
||||
_coordinate = vehicleInfo.location; |
||||
emit coordinateChanged(); |
||||
} |
||||
} |
||||
if (vehicleInfo.availableFlags & AltitudeAvailable) { |
||||
if (!(qIsNaN(vehicleInfo.altitude) && qIsNaN(_altitude)) && !qFuzzyCompare(vehicleInfo.altitude, _altitude)) { |
||||
_altitude = vehicleInfo.altitude; |
||||
emit altitudeChanged(); |
||||
} |
||||
} |
||||
if (vehicleInfo.availableFlags & HeadingAvailable) { |
||||
if (!(qIsNaN(vehicleInfo.heading) && qIsNaN(_heading)) && !qFuzzyCompare(vehicleInfo.heading, _heading)) { |
||||
_heading = vehicleInfo.heading; |
||||
emit headingChanged(); |
||||
} |
||||
} |
||||
if (vehicleInfo.availableFlags & AlertAvailable) { |
||||
if (vehicleInfo.alert != _alert) { |
||||
_alert = vehicleInfo.alert; |
||||
emit alertChanged(); |
||||
} |
||||
} |
||||
_lastUpdateTimer.restart(); |
||||
} |
||||
|
||||
bool ADSBVehicle::expired() |
||||
{ |
||||
return _lastUpdateTimer.hasExpired(expirationTimeoutMs); |
||||
} |
||||
@ -0,0 +1,188 @@
@@ -0,0 +1,188 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ADSBVehicleManager.h" |
||||
#include "QGCLoggingCategory.h" |
||||
#include "QGCApplication.h" |
||||
#include "SettingsManager.h" |
||||
#include "ADSBVehicleManagerSettings.h" |
||||
|
||||
#include <QDebug> |
||||
|
||||
ADSBVehicleManager::ADSBVehicleManager(QGCApplication* app, QGCToolbox* toolbox) |
||||
: QGCTool(app, toolbox) |
||||
{ |
||||
} |
||||
|
||||
void ADSBVehicleManager::setToolbox(QGCToolbox* toolbox) |
||||
{ |
||||
QGCTool::setToolbox(toolbox); |
||||
|
||||
connect(&_adsbVehicleCleanupTimer, &QTimer::timeout, this, &ADSBVehicleManager::_cleanupStaleVehicles); |
||||
_adsbVehicleCleanupTimer.setSingleShot(false); |
||||
_adsbVehicleCleanupTimer.start(1000); |
||||
|
||||
ADSBVehicleManagerSettings* settings = qgcApp()->toolbox()->settingsManager()->adsbVehicleManagerSettings(); |
||||
if (settings->adsbServerConnectEnabled()->rawValue().toBool()) { |
||||
_tcpLink = new ADSBTCPLink(this); |
||||
connect(_tcpLink, &ADSBTCPLink::adsbVehicleUpdate, this, &ADSBVehicleManager::adsbVehicleUpdate, Qt::QueuedConnection); |
||||
} |
||||
} |
||||
|
||||
void ADSBVehicleManager::_cleanupStaleVehicles() |
||||
{ |
||||
// Remove all expired ADSB vehicles
|
||||
for (int i=_adsbVehicles.count()-1; i>=0; i--) { |
||||
ADSBVehicle* adsbVehicle = _adsbVehicles.value<ADSBVehicle*>(i); |
||||
if (adsbVehicle->expired()) { |
||||
_adsbVehicles.removeAt(i); |
||||
adsbVehicle->deleteLater(); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void ADSBVehicleManager::adsbVehicleUpdate(const ADSBVehicle::VehicleInfo_t vehicleInfo) |
||||
{ |
||||
uint32_t icaoAddress = vehicleInfo.icaoAddress; |
||||
|
||||
if (_adsbICAOMap.contains(icaoAddress)) { |
||||
_adsbICAOMap[icaoAddress]->update(vehicleInfo); |
||||
} else { |
||||
if (vehicleInfo.availableFlags & ADSBVehicle::LocationAvailable) { |
||||
ADSBVehicle* adsbVehicle = new ADSBVehicle(vehicleInfo, this); |
||||
_adsbICAOMap[icaoAddress] = adsbVehicle; |
||||
_adsbVehicles.append(adsbVehicle); |
||||
} |
||||
} |
||||
} |
||||
|
||||
ADSBTCPLink::ADSBTCPLink(QObject* parent) |
||||
: QThread(parent) |
||||
{ |
||||
_settings = qgcApp()->toolbox()->settingsManager()->adsbVehicleManagerSettings(); |
||||
moveToThread(this); |
||||
start(); |
||||
} |
||||
|
||||
ADSBTCPLink::~ADSBTCPLink(void) |
||||
{ |
||||
if (_socket) { |
||||
_socket->disconnectFromHost(); |
||||
_socket->waitForDisconnected(); |
||||
_socket->deleteLater(); |
||||
_socket = nullptr; |
||||
} |
||||
quit(); |
||||
wait(); |
||||
} |
||||
|
||||
void ADSBTCPLink::run(void) |
||||
{ |
||||
_hardwareConnect(); |
||||
exec(); |
||||
} |
||||
|
||||
void ADSBTCPLink::_hardwareConnect() |
||||
{ |
||||
_socket = new QTcpSocket(); |
||||
|
||||
QObject::connect(_socket, &QTcpSocket::readyRead, this, &ADSBTCPLink::_readBytes); |
||||
|
||||
_socket->connectToHost(_settings->adsbServerHostAddress()->rawValue().toString(), _settings->adsbServerPort()->rawValue().toInt()); |
||||
|
||||
// Give the socket a second to connect to the other side otherwise error out
|
||||
if (!_socket->waitForConnected(1000)) { |
||||
qCDebug(ADSBVehicleManagerLog) << "ADSB Socket failed to connect"; |
||||
delete _socket; |
||||
_socket = nullptr; |
||||
return; |
||||
} |
||||
|
||||
qCDebug(ADSBVehicleManagerLog) << "ADSB Socket connected"; |
||||
} |
||||
|
||||
void ADSBTCPLink::_readBytes(void) |
||||
{ |
||||
if (_socket) { |
||||
QByteArray bytes = _socket->readLine(); |
||||
_parseLine(QString::fromLocal8Bit(bytes)); |
||||
} |
||||
} |
||||
|
||||
void ADSBTCPLink::_socketError(QAbstractSocket::SocketError socketError) |
||||
{ |
||||
QString error = _socket->errorString(); |
||||
qDebug() << _socket->errorString(); |
||||
} |
||||
|
||||
|
||||
void ADSBTCPLink::_parseLine(const QString& line) |
||||
{ |
||||
if (line.startsWith(QStringLiteral("MSG"))) { |
||||
qCDebug(ADSBVehicleManagerLog) << "ADSB SBS-1" << line; |
||||
|
||||
QStringList values = line.split(QStringLiteral(",")); |
||||
|
||||
if (values[1] == QStringLiteral("3")) { |
||||
bool icaoOk, altOk, latOk, lonOk; |
||||
|
||||
uint32_t icaoAddress = values[4].toUInt(&icaoOk, 16); |
||||
int modeCAltitude = values[11].toInt(&altOk); |
||||
double lat = values[14].toDouble(&latOk); |
||||
double lon = values[15].toDouble(&lonOk); |
||||
QString callsign = values[10]; |
||||
|
||||
if (!icaoOk || !altOk || !latOk || !lonOk) { |
||||
return; |
||||
} |
||||
if (lat == 0 && lon == 0) { |
||||
return; |
||||
} |
||||
|
||||
double altitude = modeCAltitude / 3.048; |
||||
QGeoCoordinate location(lat, lon); |
||||
|
||||
ADSBVehicle::VehicleInfo_t adsbInfo; |
||||
adsbInfo.icaoAddress = icaoAddress; |
||||
adsbInfo.callsign = callsign; |
||||
adsbInfo.location = location; |
||||
adsbInfo.altitude = altitude; |
||||
adsbInfo.availableFlags = ADSBVehicle::CallsignAvailable | ADSBVehicle::LocationAvailable | ADSBVehicle::AltitudeAvailable; |
||||
emit adsbVehicleUpdate(adsbInfo); |
||||
} else if (values[1] == QStringLiteral("4")) { |
||||
bool icaoOk, headingOk; |
||||
|
||||
uint32_t icaoAddress = values[4].toUInt(&icaoOk, 16); |
||||
double heading = values[13].toDouble(&headingOk); |
||||
|
||||
if (!icaoOk || !headingOk) { |
||||
return; |
||||
} |
||||
|
||||
ADSBVehicle::VehicleInfo_t adsbInfo; |
||||
adsbInfo.icaoAddress = icaoAddress; |
||||
adsbInfo.heading = heading; |
||||
adsbInfo.availableFlags = ADSBVehicle::HeadingAvailable; |
||||
emit adsbVehicleUpdate(adsbInfo); |
||||
} else if (values[1] == QStringLiteral("1")) { |
||||
bool icaoOk; |
||||
|
||||
uint32_t icaoAddress = values[4].toUInt(&icaoOk, 16); |
||||
if (!icaoOk) { |
||||
return; |
||||
} |
||||
|
||||
ADSBVehicle::VehicleInfo_t adsbInfo; |
||||
adsbInfo.icaoAddress = icaoAddress; |
||||
adsbInfo.callsign = values[10]; |
||||
adsbInfo.availableFlags = ADSBVehicle::CallsignAvailable; |
||||
emit adsbVehicleUpdate(adsbInfo); |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,73 @@
@@ -0,0 +1,73 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "QGCToolbox.h" |
||||
#include "QmlObjectListModel.h" |
||||
#include "ADSBVehicle.h" |
||||
|
||||
#include <QThread> |
||||
#include <QTcpSocket> |
||||
#include <QTimer> |
||||
#include <QGeoCoordinate> |
||||
|
||||
class ADSBVehicleManagerSettings; |
||||
|
||||
class ADSBTCPLink : public QThread |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
ADSBTCPLink(QObject* parent); |
||||
~ADSBTCPLink(); |
||||
|
||||
signals: |
||||
void adsbVehicleUpdate(const ADSBVehicle::VehicleInfo_t vehicleInfo); |
||||
|
||||
protected: |
||||
void run(void) final; |
||||
|
||||
private slots: |
||||
void _readBytes(void); |
||||
void _socketError(QAbstractSocket::SocketError socketError); |
||||
|
||||
private: |
||||
void _hardwareConnect(void); |
||||
void _parseLine(const QString& line); |
||||
|
||||
QTcpSocket* _socket = nullptr; |
||||
ADSBVehicleManagerSettings* _settings = nullptr; |
||||
}; |
||||
|
||||
class ADSBVehicleManager : public QGCTool { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
ADSBVehicleManager(QGCApplication* app, QGCToolbox* toolbox); |
||||
|
||||
Q_PROPERTY(QmlObjectListModel* adsbVehicles READ adsbVehicles CONSTANT) |
||||
|
||||
QmlObjectListModel* adsbVehicles(void) { return &_adsbVehicles; } |
||||
|
||||
// QGCTool overrides
|
||||
void setToolbox(QGCToolbox* toolbox) final; |
||||
|
||||
public slots: |
||||
void adsbVehicleUpdate(const ADSBVehicle::VehicleInfo_t vehicleInfo); |
||||
|
||||
private slots: |
||||
void _cleanupStaleVehicles(void); |
||||
|
||||
private: |
||||
QmlObjectListModel _adsbVehicles; |
||||
QMap<uint32_t, ADSBVehicle*> _adsbICAOMap; |
||||
QTimer _adsbVehicleCleanupTimer; |
||||
ADSBTCPLink* _tcpLink = nullptr; |
||||
}; |
||||
@ -0,0 +1,24 @@
@@ -0,0 +1,24 @@
|
||||
[ |
||||
{ |
||||
"name": "adsbServerConnectEnabled", |
||||
"shortDescription": "Connect to ADSB SBS server", |
||||
"longDescription": "Connect to ADSB SBS-1 server using specified address/port", |
||||
"type": "bool", |
||||
"defaultValue": false, |
||||
"qgcRebootRequired": true |
||||
}, |
||||
{ |
||||
"name": "adsbServerHostAddress", |
||||
"shortDescription": "Host address", |
||||
"type": "string", |
||||
"defaultValue": "127.0.0.1", |
||||
"qgcRebootRequired": true |
||||
}, |
||||
{ |
||||
"name": "adsbServerPort", |
||||
"shortDescription": "Server port", |
||||
"type": "string", |
||||
"defaultValue": 30003, |
||||
"qgcRebootRequired": true |
||||
} |
||||
] |
||||
@ -0,0 +1,22 @@
@@ -0,0 +1,22 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ADSBVehicleManagerSettings.h" |
||||
|
||||
#include <QQmlEngine> |
||||
#include <QtQml> |
||||
|
||||
DECLARE_SETTINGGROUP(ADSBVehicleManager, "ADSBVehicleManager") |
||||
{ |
||||
qmlRegisterUncreatableType<ADSBVehicleManagerSettings>("QGroundControl.SettingsManager", 1, 0, "ADSBVehicleManagerSettings", "Reference only"); |
||||
} |
||||
|
||||
DECLARE_SETTINGSFACT(ADSBVehicleManagerSettings, adsbServerConnectEnabled) |
||||
DECLARE_SETTINGSFACT(ADSBVehicleManagerSettings, adsbServerHostAddress) |
||||
DECLARE_SETTINGSFACT(ADSBVehicleManagerSettings, adsbServerPort) |
||||
@ -0,0 +1,24 @@
@@ -0,0 +1,24 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "SettingsGroup.h" |
||||
|
||||
class ADSBVehicleManagerSettings : public SettingsGroup |
||||
{ |
||||
Q_OBJECT |
||||
public: |
||||
ADSBVehicleManagerSettings(QObject* parent = nullptr); |
||||
DEFINE_SETTING_NAME_GROUP() |
||||
|
||||
DEFINE_SETTINGFACT(adsbServerConnectEnabled) |
||||
DEFINE_SETTINGFACT(adsbServerHostAddress) |
||||
DEFINE_SETTINGFACT(adsbServerPort) |
||||
}; |
||||
@ -1,98 +0,0 @@
@@ -1,98 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
* |
||||
* QGroundControl is licensed according to the terms in the file |
||||
* COPYING.md in the root of the source code directory. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ADSBVehicle.h" |
||||
|
||||
#include <QDebug> |
||||
#include <QtMath> |
||||
|
||||
ADSBVehicle::ADSBVehicle(mavlink_adsb_vehicle_t& adsbVehicle, QObject* parent) |
||||
: QObject (parent) |
||||
, _icaoAddress (adsbVehicle.ICAO_address) |
||||
, _callsign (adsbVehicle.callsign) |
||||
, _altitude (NAN) |
||||
, _heading (NAN) |
||||
, _alert (false) |
||||
{ |
||||
if (!(adsbVehicle.flags & ADSB_FLAGS_VALID_COORDS)) { |
||||
qWarning() << "At least coords must be valid"; |
||||
return; |
||||
} |
||||
update(adsbVehicle); |
||||
} |
||||
|
||||
ADSBVehicle::ADSBVehicle(const QGeoCoordinate& location, float heading, bool alert, QObject* parent) |
||||
: QObject(parent) |
||||
, _icaoAddress(0) |
||||
, _alert(alert) |
||||
{ |
||||
update(alert, location, heading); |
||||
} |
||||
|
||||
void ADSBVehicle::update(bool alert, const QGeoCoordinate& location, float heading) |
||||
{ |
||||
_coordinate = location; |
||||
_altitude = location.altitude(); |
||||
_heading = heading; |
||||
_alert = alert; |
||||
emit coordinateChanged(); |
||||
emit altitudeChanged(); |
||||
emit headingChanged(); |
||||
emit alertChanged(); |
||||
_lastUpdateTimer.restart(); |
||||
} |
||||
|
||||
void ADSBVehicle::update(mavlink_adsb_vehicle_t& adsbVehicle) |
||||
{ |
||||
if (_icaoAddress != adsbVehicle.ICAO_address) { |
||||
qWarning() << "ICAO address mismatch expected:actual" << _icaoAddress << adsbVehicle.ICAO_address; |
||||
return; |
||||
} |
||||
|
||||
if (!(adsbVehicle.flags & ADSB_FLAGS_VALID_COORDS)) { |
||||
return; |
||||
} |
||||
|
||||
QString currCallsign(adsbVehicle.callsign); |
||||
|
||||
if (currCallsign != _callsign) { |
||||
_callsign = currCallsign; |
||||
emit callsignChanged(); |
||||
} |
||||
|
||||
QGeoCoordinate newCoordinate(adsbVehicle.lat / 1e7, adsbVehicle.lon / 1e7); |
||||
if (newCoordinate != _coordinate) { |
||||
_coordinate = newCoordinate; |
||||
emit coordinateChanged(); |
||||
} |
||||
|
||||
double newAltitude = NAN; |
||||
if (adsbVehicle.flags & ADSB_FLAGS_VALID_ALTITUDE) { |
||||
newAltitude = (double)adsbVehicle.altitude / 1e3; |
||||
} |
||||
if (!(qIsNaN(newAltitude) && qIsNaN(_altitude)) && !qFuzzyCompare(newAltitude, _altitude)) { |
||||
_altitude = newAltitude; |
||||
emit altitudeChanged(); |
||||
} |
||||
|
||||
double newHeading = NAN; |
||||
if (adsbVehicle.flags & ADSB_FLAGS_VALID_HEADING) { |
||||
newHeading = (double)adsbVehicle.heading / 100.0; |
||||
} |
||||
if (!(qIsNaN(newHeading) && qIsNaN(_heading)) && !qFuzzyCompare(newHeading, _heading)) { |
||||
_heading = newHeading; |
||||
emit headingChanged(); |
||||
} |
||||
_lastUpdateTimer.restart(); |
||||
} |
||||
|
||||
bool ADSBVehicle::expired() |
||||
{ |
||||
return _lastUpdateTimer.hasExpired(expirationTimeoutMs); |
||||
} |
||||
Loading…
Reference in new issue