You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
299 lines
8.5 KiB
299 lines
8.5 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
// Copyright (C) 2026 Jon Beniston, M7RCE <jon@beniston.com> // |
|
// Some code by AI // |
|
// // |
|
// This program is free software; you can redistribute it and/or modify // |
|
// it under the terms of the GNU General Public License as published by // |
|
// the Free Software Foundation as version 3 of the License, or // |
|
// (at your option) any later version. // |
|
// // |
|
// This program is distributed in the hope that it will be useful, // |
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of // |
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // |
|
// GNU General Public License V3 for more details. // |
|
// // |
|
// You should have received a copy of the GNU General Public License // |
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. // |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
#include <cmath> |
|
#include <algorithm> |
|
|
|
#include <QMap> |
|
#include <QStringList> |
|
|
|
#include "util/crc.h" |
|
|
|
#include "droneid.h" |
|
|
|
uint16_t DroneIDMessage::getU16(const uint8_t *p) |
|
{ |
|
return p[0] | (p[1] << 8); |
|
} |
|
|
|
int16_t DroneIDMessage::getS16(const uint8_t *p) |
|
{ |
|
return (int16_t) getU16(p); |
|
} |
|
|
|
int32_t DroneIDMessage::getS32(const uint8_t *p) |
|
{ |
|
return (int32_t) ((uint32_t) p[0] | ((uint32_t) p[1] << 8) | ((uint32_t) p[2] << 16) | ((uint32_t) p[3] << 24)); |
|
} |
|
|
|
uint64_t DroneIDMessage::getU64(const uint8_t *p) |
|
{ |
|
uint64_t v = 0; |
|
for (int i = 7; i >= 0; i--) { |
|
v = (v << 8) | p[i]; |
|
} |
|
return v; |
|
} |
|
|
|
// Positions are transmitted in radians * 1e7 |
|
double DroneIDMessage::toDegrees(int32_t v) |
|
{ |
|
return (v / 1.0e7) * (180.0 / M_PI); |
|
} |
|
|
|
QString DroneIDMessage::getString(const uint8_t *p, int maxLength) |
|
{ |
|
int len = 0; |
|
while ((len < maxLength) && (p[len] != 0)) { |
|
len++; |
|
} |
|
return QString::fromLatin1((const char *) p, len).trimmed(); |
|
} |
|
|
|
uint16_t DroneIDMessage::calcCRC(const uint8_t *bytes, int length) |
|
{ |
|
crc16droneid crc; |
|
crc.calculate(bytes, length); |
|
return crc.get(); |
|
} |
|
|
|
int DroneIDMessage::messageLength(const QByteArray& bytes) |
|
{ |
|
if (bytes.size() < 3) { |
|
return -1; |
|
} |
|
int length = (uint8_t) bytes[0] + 3; |
|
if (length > bytes.size()) { |
|
return -1; |
|
} |
|
return length; |
|
} |
|
|
|
DroneIDMessage* DroneIDMessage::decode(const QByteArray& bytes) |
|
{ |
|
int length = messageLength(bytes); |
|
if (length < 0) { |
|
return nullptr; |
|
} |
|
|
|
const uint8_t *p = (const uint8_t *) bytes.data(); |
|
uint8_t type = p[1]; |
|
|
|
if (type == FlightInformation) |
|
{ |
|
// Versions 1 to 3 seen |
|
if ((length < m_flightInfoLength) || (p[2] < 1) || (p[2] > 3)) { |
|
return nullptr; |
|
} |
|
} |
|
else if (type == FlightPurpose) |
|
{ |
|
if (length < 2 + 16 + 2) { |
|
return nullptr; |
|
} |
|
} |
|
else |
|
{ |
|
return nullptr; |
|
} |
|
|
|
DroneIDMessage *msg = new DroneIDMessage(); |
|
|
|
msg->m_packetLength = p[0]; |
|
msg->m_type = type; |
|
msg->m_crc = getU16(&p[length - 2]); |
|
msg->m_crcValid = calcCRC(p, length - 2) == msg->m_crc; |
|
|
|
if (type == FlightInformation) |
|
{ |
|
msg->m_version = p[2]; |
|
msg->m_sequence = getU16(&p[3]); |
|
msg->m_stateInfo = getU16(&p[5]); |
|
msg->m_serial = getString(&p[7], 16); |
|
msg->m_longitude = toDegrees(getS32(&p[23])); |
|
msg->m_latitude = toDegrees(getS32(&p[27])); |
|
// Units of altitude and height are not documented. Sources differ, so these need confirming with real data. |
|
msg->m_altitude = getS16(&p[31]); |
|
msg->m_height = getS16(&p[33]) / 10.0f; |
|
msg->m_vNorth = getS16(&p[35]) / 100.0f; |
|
msg->m_vEast = getS16(&p[37]) / 100.0f; |
|
msg->m_vUp = getS16(&p[39]) / 100.0f; |
|
msg->m_yaw = getS16(&p[41]) / 100.0f; |
|
uint64_t gpsTime = getU64(&p[43]); |
|
if (gpsTime != 0) { |
|
msg->m_gpsTime = QDateTime::fromMSecsSinceEpoch((qint64) gpsTime).toUTC(); |
|
} |
|
msg->m_pilotLatitude = toDegrees(getS32(&p[51])); |
|
msg->m_pilotLongitude = toDegrees(getS32(&p[55])); |
|
msg->m_homeLongitude = toDegrees(getS32(&p[59])); |
|
msg->m_homeLatitude = toDegrees(getS32(&p[63])); |
|
msg->m_productType = p[67]; |
|
int uuidLength = std::min((int) p[68], 20); |
|
msg->m_uuid = getString(&p[69], uuidLength); |
|
} |
|
else |
|
{ |
|
msg->m_serial = getString(&p[2], 16); |
|
msg->m_purpose = bytes.mid(18, length - 18 - 2); |
|
} |
|
|
|
return msg; |
|
} |
|
|
|
bool DroneIDMessage::validPosition(double latitude, double longitude) |
|
{ |
|
return (latitude != 0.0) && (longitude != 0.0) |
|
&& (std::abs(latitude) <= 90.0) && (std::abs(longitude) <= 180.0); |
|
} |
|
|
|
bool DroneIDMessage::hasPosition() const |
|
{ |
|
return validPosition(m_latitude, m_longitude); |
|
} |
|
|
|
bool DroneIDMessage::hasPilotPosition() const |
|
{ |
|
return validPosition(m_pilotLatitude, m_pilotLongitude); |
|
} |
|
|
|
bool DroneIDMessage::hasHomePosition() const |
|
{ |
|
return validPosition(m_homeLatitude, m_homeLongitude); |
|
} |
|
|
|
float DroneIDMessage::getSpeed() const |
|
{ |
|
return std::sqrt(m_vNorth * m_vNorth + m_vEast * m_vEast); |
|
} |
|
|
|
float DroneIDMessage::getCourse() const |
|
{ |
|
float course = std::atan2(m_vEast, m_vNorth) * (180.0f / (float) M_PI); |
|
if (course < 0.0f) { |
|
course += 360.0f; |
|
} |
|
return course; |
|
} |
|
|
|
QString DroneIDMessage::getStateInfoString() const |
|
{ |
|
QStringList flags; |
|
|
|
if (m_stateInfo & MotorsOn) { |
|
flags.append("Motors on"); |
|
} |
|
if (m_stateInfo & InAir) { |
|
flags.append("In air"); |
|
} |
|
if (m_stateInfo & GPSValid) { |
|
flags.append("GPS"); |
|
} |
|
if (m_stateInfo & HomeValid) { |
|
flags.append("Home"); |
|
} |
|
if (m_stateInfo & PrivacyEnabled) { |
|
flags.append("Privacy"); |
|
} |
|
return flags.join(", "); |
|
} |
|
|
|
QString DroneIDMessage::productTypeName(int productType) |
|
{ |
|
// Codes from DroneSecurity, Kismet and others. Codes above 70 are less well verified. |
|
static const QMap<int, QString> names = { |
|
{1, "Inspire 1"}, |
|
{2, "Phantom 3 Series"}, |
|
{3, "Phantom 3 Series Pro"}, |
|
{4, "Phantom 3 Std"}, |
|
{5, "M100"}, |
|
{6, "ACEONE"}, |
|
{7, "WKM"}, |
|
{8, "NAZA"}, |
|
{9, "A2"}, |
|
{10, "A3"}, |
|
{11, "Phantom 4"}, |
|
{12, "MG1"}, |
|
{14, "M600"}, |
|
{15, "Phantom 3 4K"}, |
|
{16, "Mavic Pro"}, |
|
{17, "Inspire 2"}, |
|
{18, "Phantom 4 Pro"}, |
|
{20, "N2"}, |
|
{21, "Spark"}, |
|
{23, "M600 Pro"}, |
|
{24, "Mavic Air"}, |
|
{25, "M200"}, |
|
{26, "Phantom 4 Series"}, |
|
{27, "Phantom 4 Adv"}, |
|
{28, "M210"}, |
|
{30, "M210 RTK"}, |
|
{31, "A3 AG"}, |
|
{32, "MG2"}, |
|
{34, "MG1A"}, |
|
{35, "Phantom 4 RTK"}, |
|
{36, "Phantom 4 Pro V2.0"}, |
|
{38, "MG1P"}, |
|
{40, "MG1P-RTK"}, |
|
{41, "Mavic 2"}, |
|
{44, "M200 V2 Series"}, |
|
{51, "Mavic 2 Enterprise"}, |
|
{53, "Mavic Mini"}, |
|
{58, "Mavic Air 2"}, |
|
{59, "P4M"}, |
|
{60, "M300 RTK"}, |
|
{61, "DJI FPV"}, |
|
{63, "Mini 2"}, |
|
{64, "AGRAS T10"}, |
|
{65, "AGRAS T30"}, |
|
{66, "Air 2S"}, |
|
{67, "M30"}, |
|
{68, "Mavic 3"}, |
|
{69, "Mavic 2 Enterprise Advanced"}, |
|
{70, "Mini SE"}, |
|
{73, "Mini 3 Pro"}, |
|
{75, "Avata"}, |
|
{77, "Mavic 3 Enterprise"}, |
|
{82, "AGRAS T25"}, |
|
{83, "AGRAS T50"}, |
|
{86, "Mavic 3 Classic"}, |
|
{87, "Mini 3"}, |
|
{88, "Mini 2 SE"}, |
|
{90, "Air 3"} |
|
}; |
|
|
|
return names.value(productType, QString("Unknown (%1)").arg(productType)); |
|
} |
|
|
|
QString DroneIDMessage::toString() const |
|
{ |
|
if (!isFlightInformation()) { |
|
return QString("Flight purpose %1").arg(m_serial); |
|
} |
|
return QString("%1 %2 seq %3 lat %4 lon %5 alt %6 height %7 pilot %8,%9 home %10,%11") |
|
.arg(getProductTypeName()) |
|
.arg(m_serial) |
|
.arg(m_sequence) |
|
.arg(m_latitude, 0, 'f', 6) |
|
.arg(m_longitude, 0, 'f', 6) |
|
.arg(m_altitude) |
|
.arg(m_height) |
|
.arg(m_pilotLatitude, 0, 'f', 6) |
|
.arg(m_pilotLongitude, 0, 'f', 6) |
|
.arg(m_homeLatitude, 0, 'f', 6) |
|
.arg(m_homeLongitude, 0, 'f', 6); |
|
}
|
|
|