Commit 0a9a2d64 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #5482 from DonLakeFlyer/ADSBTimeout

ADS-B vehicle timeout and stacked callsign
parents fb8ac8d2 8b72be79
......@@ -57,6 +57,7 @@ MapQuickItem {
}
QGCMapLabel {
id: vehicleLabel
anchors.top: parent.bottom
anchors.horizontalCenter: parent.horizontalCenter
map: _map
......@@ -66,10 +67,21 @@ MapQuickItem {
property string vehicleLabelText: visible ?
(_adsbVehicle ?
callsign + " " + QGroundControl.metersToAppSettingsDistanceUnits(altitude).toFixed(0) + " " + QGroundControl.appSettingsDistanceUnitsString :
QGroundControl.metersToAppSettingsDistanceUnits(altitude).toFixed(0) + " " + QGroundControl.appSettingsDistanceUnitsString :
(_multiVehicle ? qsTr("Vehicle %1").arg(vehicle.id) : "")) :
""
}
QGCMapLabel {
anchors.top: vehicleLabel.bottom
anchors.horizontalCenter: parent.horizontalCenter
map: _map
text: vehicleLabelText
font.pointSize: ScreenTools.smallFontPointSize
visible: _adsbVehicle ? !isNaN(altitude) : _multiVehicle
property string vehicleLabelText: visible && _adsbVehicle ? callsign : ""
}
}
}
......@@ -2631,12 +2631,20 @@ bool Vehicle::autoDisarm(void)
void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
mavlink_adsb_vehicle_t adsbVehicle;
static const int maxTimeSinceLastSeen = 15;
mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicle);
if (adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS) {
if (_adsbICAOMap.contains(adsbVehicle.ICAO_address)) {
_adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
} else {
if (adsbVehicle.tslc > maxTimeSinceLastSeen) {
ADSBVehicle* vehicle = _adsbICAOMap[adsbVehicle.ICAO_address];
_adsbVehicles.removeOne(vehicle);
_adsbICAOMap.remove(adsbVehicle.ICAO_address);
vehicle->deleteLater();
} else {
_adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
}
} else if (adsbVehicle.tslc <= maxTimeSinceLastSeen) {
ADSBVehicle* vehicle = new ADSBVehicle(adsbVehicle, this);
_adsbICAOMap[adsbVehicle.ICAO_address] = vehicle;
_adsbVehicles.append(vehicle);
......
......@@ -1278,8 +1278,8 @@ void MockLink::_sendADSBVehicles(void)
_mavlinkChannel,
&responseMsg,
12345, // ICAO address
(_vehicleLatitude + 0.001) * qPow(10.0, 7.0),
(_vehicleLongitude + 0.001) * qPow(10.0, 7.0),
(_vehicleLatitude + 0.001) * 1e7,
(_vehicleLongitude + 0.001) * 1e7,
ADSB_ALTITUDE_TYPE_GEOMETRIC,
100 * 1000, // Altitude in millimeters
10 * 100, // Heading in centidegress
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment