diff --git a/src/ADSB/ADSB.h b/src/ADSB/ADSB.h index b533ee701384..c408daa8b434 100644 --- a/src/ADSB/ADSB.h +++ b/src/ADSB/ADSB.h @@ -38,24 +38,32 @@ Q_ENUM_NS(MessageType) /// Enum for ADSB Info Types Available. enum AvailableInfoType { - CallsignAvailable = 1 << 0, - LocationAvailable = 1 << 1, - AltitudeAvailable = 1 << 2, - HeadingAvailable = 1 << 3, - AlertAvailable = 1 << 4, + LocationAvailable = 1 << 0, + AltitudeAvailable = 1 << 1, + HeadingAvailable = 1 << 2, + VelocityAvailable = 1 << 3, + CallsignAvailable = 1 << 4, + SquawkAvailable = 1 << 5, + VerticalVelAvailable = 1 << 6, + AlertAvailable = 1 << 7 }; Q_FLAG_NS(AvailableInfoType) Q_DECLARE_FLAGS(AvailableInfoTypes, AvailableInfoType) Q_DECLARE_OPERATORS_FOR_FLAGS(AvailableInfoTypes) struct VehicleInfo_t { - uint32_t icaoAddress; + AvailableInfoTypes availableFlags; + uint32_t icaoAddress = 0; QString callsign; QGeoCoordinate location; - double altitude; // TODO: Use Altitude in QGeoCoordinate? - double heading; - bool alert; - AvailableInfoTypes availableFlags; + double heading = 0.0; + uint16_t squawk = 0; + double velocity = 0.0; + double verticalVel = 0.0; + uint32_t lastContact = 0; + bool simulated = false; + bool baro = false; + bool alert = false; }; } // namespace ADSB diff --git a/src/ADSB/ADSBTCPLink.cc b/src/ADSB/ADSBTCPLink.cc index 022ad81fd62c..bbb21ed8e293 100644 --- a/src/ADSB/ADSBTCPLink.cc +++ b/src/ADSB/ADSBTCPLink.cc @@ -24,24 +24,26 @@ ADSBTCPLink::ADSBTCPLink(const QHostAddress &hostAddress, quint16 port, QObject , _processTimer(new QTimer(this)) { #ifdef QT_DEBUG - (void) connect(_socket, &QTcpSocket::stateChanged, this, [](QTcpSocket::SocketState state) { - switch (state) { - case QTcpSocket::UnconnectedState: - qCDebug(ADSBTCPLinkLog) << "ADSB Socket disconnected"; - break; - case QTcpSocket::SocketState::ConnectingState: - qCDebug(ADSBTCPLinkLog) << "ADSB Socket connecting..."; - break; - case QTcpSocket::SocketState::ConnectedState: - qCDebug(ADSBTCPLinkLog) << "ADSB Socket connected"; - break; - case QTcpSocket::SocketState::ClosingState: - qCDebug(ADSBTCPLinkLog) << "ADSB Socket closing..."; - break; - default: - break; - } - }, Qt::AutoConnection); + if (ADSBTCPLinkLog().isDebugEnabled()) { + (void) connect(_socket, &QTcpSocket::stateChanged, this, [](QTcpSocket::SocketState state) { + switch (state) { + case QTcpSocket::UnconnectedState: + qCDebug(ADSBTCPLinkLog) << "ADSB Socket disconnected"; + break; + case QTcpSocket::SocketState::ConnectingState: + qCDebug(ADSBTCPLinkLog) << "ADSB Socket connecting..."; + break; + case QTcpSocket::SocketState::ConnectedState: + qCDebug(ADSBTCPLinkLog) << "ADSB Socket connected"; + break; + case QTcpSocket::SocketState::ClosingState: + qCDebug(ADSBTCPLinkLog) << "ADSB Socket closing..."; + break; + default: + break; + } + }, Qt::AutoConnection); + } #endif (void) QObject::connect(_socket, &QTcpSocket::errorOccurred, this, [this](QTcpSocket::SocketError error) { @@ -209,10 +211,9 @@ void ADSBTCPLink::_parseAndEmitLocation(ADSB::VehicleInfo_t &adsbInfo, const QSt } const double altitude = modeCAltitude * 0.3048; - const QGeoCoordinate location(lat, lon); + const QGeoCoordinate location(lat, lon, altitude); adsbInfo.location = location; - adsbInfo.altitude = altitude; adsbInfo.alert = (alert == 1); adsbInfo.availableFlags = ADSB::LocationAvailable | ADSB::AltitudeAvailable | ADSB::AlertAvailable; diff --git a/src/ADSB/ADSBVehicle.cc b/src/ADSB/ADSBVehicle.cc index b17ab35e5340..e9dd368214fe 100644 --- a/src/ADSB/ADSBVehicle.cc +++ b/src/ADSB/ADSBVehicle.cc @@ -38,23 +38,17 @@ void ADSBVehicle::update(const ADSB::VehicleInfo_t &vehicleInfo) qCDebug(ADSBVehicleLog) << "Updating" << QStringLiteral("%1 Flags: %2").arg(vehicleInfo.icaoAddress, 0, 16).arg(vehicleInfo.availableFlags, 0, 2); - if (vehicleInfo.availableFlags & ADSB::CallsignAvailable) { - if (vehicleInfo.callsign != callsign()) { - _info.callsign = vehicleInfo.callsign; - emit callsignChanged(); - } - } - if (vehicleInfo.availableFlags & ADSB::LocationAvailable) { - if (vehicleInfo.location != coordinate()) { - _info.location = vehicleInfo.location; + if (!QGC::fuzzyCompare(vehicleInfo.location.latitude(), coordinate().latitude()) || !QGC::fuzzyCompare(vehicleInfo.location.longitude(), coordinate().longitude())) { + _info.location.setLatitude(vehicleInfo.location.latitude()); + _info.location.setLongitude(vehicleInfo.location.longitude()); emit coordinateChanged(); } } if (vehicleInfo.availableFlags & ADSB::AltitudeAvailable) { - if (!QGC::fuzzyCompare(vehicleInfo.altitude, altitude())) { - _info.altitude = vehicleInfo.altitude; + if (!QGC::fuzzyCompare(vehicleInfo.location.altitude(), altitude())) { + _info.location.setAltitude(vehicleInfo.location.altitude()); emit altitudeChanged(); } } @@ -66,6 +60,34 @@ void ADSBVehicle::update(const ADSB::VehicleInfo_t &vehicleInfo) } } + if (vehicleInfo.availableFlags & ADSB::VelocityAvailable) { + if (!QGC::fuzzyCompare(vehicleInfo.velocity, velocity())) { + _info.velocity = vehicleInfo.velocity; + emit velocityChanged(); + } + } + + if (vehicleInfo.availableFlags & ADSB::CallsignAvailable) { + if (vehicleInfo.callsign != callsign()) { + _info.callsign = vehicleInfo.callsign; + emit callsignChanged(); + } + } + + if (vehicleInfo.availableFlags & ADSB::SquawkAvailable) { + if (!QGC::fuzzyCompare(vehicleInfo.velocity, squawk())) { + _info.squawk = vehicleInfo.squawk; + emit squawkChanged(); + } + } + + if (vehicleInfo.availableFlags & ADSB::VerticalVelAvailable) { + if (!QGC::fuzzyCompare(vehicleInfo.verticalVel, verticalVel())) { + _info.verticalVel = vehicleInfo.verticalVel; + emit verticalVelChanged(); + } + } + if (vehicleInfo.availableFlags & ADSB::AlertAvailable) { if (vehicleInfo.alert != alert()) { _info.alert = vehicleInfo.alert; diff --git a/src/ADSB/ADSBVehicle.h b/src/ADSB/ADSBVehicle.h index 2633e95da189..c607f8c285ad 100644 --- a/src/ADSB/ADSBVehicle.h +++ b/src/ADSB/ADSBVehicle.h @@ -25,12 +25,15 @@ class ADSBVehicle : public QObject Q_OBJECT QML_ELEMENT - Q_PROPERTY(uint icaoAddress READ icaoAddress CONSTANT) - Q_PROPERTY(QString callsign READ callsign NOTIFY callsignChanged) - Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) - Q_PROPERTY(double altitude READ altitude NOTIFY altitudeChanged) - Q_PROPERTY(double heading READ heading NOTIFY headingChanged) - Q_PROPERTY(bool alert READ alert NOTIFY alertChanged) + Q_PROPERTY(uint icaoAddress READ icaoAddress CONSTANT) + Q_PROPERTY(QString callsign READ callsign NOTIFY callsignChanged) + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) + Q_PROPERTY(double altitude READ altitude NOTIFY altitudeChanged) + Q_PROPERTY(double heading READ heading NOTIFY headingChanged) + Q_PROPERTY(double velocity READ velocity NOTIFY velocityChanged) + Q_PROPERTY(double verticalVel READ verticalVel NOTIFY verticalVelChanged) + Q_PROPERTY(uint16_t squawk READ squawk NOTIFY squawkChanged) + Q_PROPERTY(bool alert READ alert NOTIFY alertChanged) public: explicit ADSBVehicle(const ADSB::VehicleInfo_t &vehicleInfo, QObject *parent = nullptr); @@ -39,8 +42,11 @@ class ADSBVehicle : public QObject uint32_t icaoAddress() const { return _info.icaoAddress; } QString callsign() const { return _info.callsign; } QGeoCoordinate coordinate() const { return _info.location; } - double altitude() const { return _info.altitude; } + double altitude() const { return _info.location.altitude(); } double heading() const { return _info.heading; } + double velocity() const { return _info.velocity; } + double verticalVel() const { return _info.verticalVel; } + uint16_t squawk() const { return _info.squawk; } bool alert() const { return _info.alert; } bool expired() const { return _lastUpdateTimer.hasExpired(_expirationTimeoutMs); } void update(const ADSB::VehicleInfo_t &vehicleInfo); @@ -50,6 +56,9 @@ class ADSBVehicle : public QObject void callsignChanged(); void altitudeChanged(); void headingChanged(); + void velocityChanged(); + void verticalVelChanged(); + void squawkChanged(); void alertChanged(); private: diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index aad585699320..a540cdd86705 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3292,33 +3292,94 @@ void Vehicle::_handleADSBVehicle(const mavlink_message_t &message) mavlink_adsb_vehicle_t adsbVehicleMsg; mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicleMsg); - static constexpr int maxTimeSinceLastSeen = 15; + static constexpr uint8_t maxTimeSinceLastSeen = 15; - if ((adsbVehicleMsg.flags & ADSB_FLAGS_VALID_COORDS) && (adsbVehicleMsg.tslc <= maxTimeSinceLastSeen)) { - ADSB::VehicleInfo_t vehicleInfo; + if (adsbVehicleMsg.tslc > maxTimeSinceLastSeen) { + return; + } + + ADSB::VehicleInfo_t vehicleInfo; + + vehicleInfo.availableFlags = ADSB::AvailableInfoTypes::fromInt(0); - vehicleInfo.availableFlags = ADSB::AvailableInfoTypes::fromInt(0); - vehicleInfo.icaoAddress = adsbVehicleMsg.ICAO_address; + vehicleInfo.icaoAddress = adsbVehicleMsg.ICAO_address; + vehicleInfo.lastContact = adsbVehicleMsg.tslc; + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_COORDS) { + vehicleInfo.availableFlags |= ADSB::LocationAvailable; vehicleInfo.location.setLatitude(adsbVehicleMsg.lat / 1e7); vehicleInfo.location.setLongitude(adsbVehicleMsg.lon / 1e7); - vehicleInfo.availableFlags |= ADSB::LocationAvailable; + } + + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_ALTITUDE) { + vehicleInfo.availableFlags |= ADSB::AltitudeAvailable; + vehicleInfo.location.setAltitude(adsbVehicleMsg.altitude / 1e3); + } + + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_HEADING) { + vehicleInfo.availableFlags |= ADSB::HeadingAvailable; + vehicleInfo.heading = adsbVehicleMsg.heading / 1e2; + } + + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_VELOCITY) { + vehicleInfo.availableFlags |= ADSB::VelocityAvailable; + vehicleInfo.velocity = adsbVehicleMsg.hor_velocity / 1e2; + } - vehicleInfo.callsign = adsbVehicleMsg.callsign; + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_CALLSIGN) { vehicleInfo.availableFlags |= ADSB::CallsignAvailable; + vehicleInfo.callsign = QString::fromLatin1(adsbVehicleMsg.callsign, sizeof(adsbVehicleMsg.callsign)); + } - if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_ALTITUDE) { - vehicleInfo.altitude = adsbVehicleMsg.altitude / 1e3; - vehicleInfo.availableFlags |= ADSB::AltitudeAvailable; - } + if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_SQUAWK) { + vehicleInfo.availableFlags |= ADSB::SquawkAvailable; + vehicleInfo.squawk = adsbVehicleMsg.squawk; + } - if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_HEADING) { - vehicleInfo.heading = adsbVehicleMsg.heading / 1e2; - vehicleInfo.availableFlags |= ADSB::HeadingAvailable; - } + if (adsbVehicleMsg.flags & ADSB_FLAGS_SIMULATED) { + vehicleInfo.simulated = true; + } + + if (adsbVehicleMsg.flags & ADSB_FLAGS_VERTICAL_VELOCITY_VALID) { + vehicleInfo.availableFlags |= ADSB::VerticalVelAvailable; + vehicleInfo.verticalVel = adsbVehicleMsg.ver_velocity; + } - (void) QMetaObject::invokeMethod(ADSBVehicleManager::instance(), "adsbVehicleUpdate", Qt::AutoConnection, vehicleInfo); + if (adsbVehicleMsg.flags & ADSB_FLAGS_BARO_VALID) { + vehicleInfo.baro = true; } + + if (adsbVehicleMsg.flags & ADSB_FLAGS_SOURCE_UAT) { + + } + + /*adsbVehicleMsg.altitude_type + ADSB_ALTITUDE_TYPE_PRESSURE_QNH + ADSB_ALTITUDE_TYPE_GEOMETRIC + + adsbVehicleMsg.emitter_type + ADSB_EMITTER_TYPE_NO_INFO + ADSB_EMITTER_TYPE_LIGHT + ADSB_EMITTER_TYPE_SMALL + ADSB_EMITTER_TYPE_LARGE + ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE + ADSB_EMITTER_TYPE_HEAVY + ADSB_EMITTER_TYPE_HIGHLY_MANUV + ADSB_EMITTER_TYPE_ROTOCRAFT + ADSB_EMITTER_TYPE_UNASSIGNED + ADSB_EMITTER_TYPE_GLIDER + ADSB_EMITTER_TYPE_LIGHTER_AIR + ADSB_EMITTER_TYPE_PARACHUTE + ADSB_EMITTER_TYPE_ULTRA_LIGHT + ADSB_EMITTER_TYPE_UNASSIGNED2 + ADSB_EMITTER_TYPE_UAV + ADSB_EMITTER_TYPE_SPACE + ADSB_EMITTER_TYPE_UNASSGINED3 + ADSB_EMITTER_TYPE_EMERGENCY_SURFACE + ADSB_EMITTER_TYPE_SERVICE_SURFACE + ADSB_EMITTER_TYPE_POINT_OBSTACLE*/ + + (void) QMetaObject::invokeMethod(ADSBVehicleManager::instance(), "adsbVehicleUpdate", Qt::AutoConnection, vehicleInfo); } void Vehicle::_updateDistanceHeadingToHome() diff --git a/test/ADSB/ADSBTest.cc b/test/ADSB/ADSBTest.cc index 8475face1a13..94083e0a3ab5 100644 --- a/test/ADSB/ADSBTest.cc +++ b/test/ADSB/ADSBTest.cc @@ -13,8 +13,7 @@ void ADSBTest::_adsbVehicleTest() ADSB::VehicleInfo_t vehicleInfo; vehicleInfo.icaoAddress = 1; vehicleInfo.callsign = QStringLiteral("1"); - vehicleInfo.location = QGeoCoordinate(1., 1.); - vehicleInfo.altitude = 1.; + vehicleInfo.location = QGeoCoordinate(1., 1., 1.); vehicleInfo.heading = 1.; vehicleInfo.alert = false; vehicleInfo.availableFlags = ADSB::CallsignAvailable; @@ -79,8 +78,7 @@ void ADSBTest::_adsbVehicleManagerTest() ADSB::VehicleInfo_t vehicleInfo; vehicleInfo.icaoAddress = 1; vehicleInfo.callsign = QStringLiteral("1"); - vehicleInfo.location = QGeoCoordinate(1., 1.); - vehicleInfo.altitude = 1.; + vehicleInfo.location = QGeoCoordinate(1., 1., 1.); vehicleInfo.heading = 1.; vehicleInfo.alert = false; vehicleInfo.availableFlags = ADSB::LocationAvailable;