diff --git a/src/Geo/QGCGeo.cc b/src/Geo/QGCGeo.cc index e45d5d567ce5f1fdaa754547f13bcd6cd975f760..9621b7572ee886206935eae66ab0b7c7012e2b22 100644 --- a/src/Geo/QGCGeo.cc +++ b/src/Geo/QGCGeo.cc @@ -125,7 +125,15 @@ QString convertGeoToMGRS(const QGeoCoordinate& coord) mgrs = ""; } - return QString::fromStdString(mgrs); + QString qstr = QString::fromStdString(mgrs); + for (int i = qstr.length() - 1; i >= 0; i--) { + if (!qstr.at(i).isDigit()) { + int l = (qstr.length() - i) / 2; + return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l); + } + } + + return qstr; } bool convertMGRSToGeo(QString mgrs, QGeoCoordinate& coord) diff --git a/src/Vehicle/GPSFact.json b/src/Vehicle/GPSFact.json index d9b3c04af2d14e172bea50aae4d738a540fcc1f9..3f7fe68437f25369a8b5a509c9648b397fd30d5f 100644 --- a/src/Vehicle/GPSFact.json +++ b/src/Vehicle/GPSFact.json @@ -11,6 +11,11 @@ "type": "double", "decimalPlaces": 7 }, +{ + "name": "mgrs", + "shortDescription": "MGRS Position", + "type": "string" +}, { "name": "hdop", "shortDescription": "HDOP", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 55eee91eaaea5891aa20579fffef24b13ff40a52..1cfab0c368f69bc31ff22945975880c4265f42e4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -44,6 +44,7 @@ #include "PositionManager.h" #include "VehicleObjectAvoidance.h" #include "TrajectoryPoints.h" +#include "QGCGeo.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" @@ -1175,6 +1176,7 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7); _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7); + _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(gpsRawInt.lat * 1e-7, gpsRawInt.lon * 1e-7))); _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible); _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? std::numeric_limits::quiet_NaN() : gpsRawInt.eph / 100.0); _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? std::numeric_limits::quiet_NaN() : gpsRawInt.epv / 100.0); @@ -1248,6 +1250,7 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7); _gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7); + _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(highLatency2.latitude * 1e-7, highLatency2.longitude * 1e-7))); _gpsFactGroup.count()->setRawValue(0); _gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits::quiet_NaN() : highLatency2.eph / 10.0); _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits::quiet_NaN() : highLatency2.epv / 10.0); @@ -3577,6 +3580,7 @@ void Vehicle::setVtolInFwdFlight(bool vtolInFwdFlight) const char* VehicleGPSFactGroup::_latFactName = "lat"; const char* VehicleGPSFactGroup::_lonFactName = "lon"; +const char* VehicleGPSFactGroup::_mgrsFactName = "mgrs"; const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; @@ -3587,6 +3591,7 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent) , _latFact (0, _latFactName, FactMetaData::valueTypeDouble) , _lonFact (0, _lonFactName, FactMetaData::valueTypeDouble) + , _mgrsFact (0, _mgrsFactName, FactMetaData::valueTypeString) , _hdopFact (0, _hdopFactName, FactMetaData::valueTypeDouble) , _vdopFact (0, _vdopFactName, FactMetaData::valueTypeDouble) , _courseOverGroundFact (0, _courseOverGroundFactName, FactMetaData::valueTypeDouble) @@ -3595,6 +3600,7 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) { _addFact(&_latFact, _latFactName); _addFact(&_lonFact, _lonFactName); + _addFact(&_mgrsFact, _mgrsFactName); _addFact(&_hdopFact, _hdopFactName); _addFact(&_vdopFact, _vdopFactName); _addFact(&_courseOverGroundFact, _courseOverGroundFactName); @@ -3603,6 +3609,7 @@ VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) _latFact.setRawValue(std::numeric_limits::quiet_NaN()); _lonFact.setRawValue(std::numeric_limits::quiet_NaN()); + _mgrsFact.setRawValue(""); _hdopFact.setRawValue(std::numeric_limits::quiet_NaN()); _vdopFact.setRawValue(std::numeric_limits::quiet_NaN()); _courseOverGroundFact.setRawValue(std::numeric_limits::quiet_NaN()); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1407941c0e364b6a18ca9dd0565ae54e4df6ca4a..a3e64e7628ac03558100e29366fc3c41a48366c8 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -217,6 +217,7 @@ public: Q_PROPERTY(Fact* lat READ lat CONSTANT) Q_PROPERTY(Fact* lon READ lon CONSTANT) + Q_PROPERTY(Fact* mgrs READ mgrs CONSTANT) Q_PROPERTY(Fact* hdop READ hdop CONSTANT) Q_PROPERTY(Fact* vdop READ vdop CONSTANT) Q_PROPERTY(Fact* courseOverGround READ courseOverGround CONSTANT) @@ -225,6 +226,7 @@ public: Fact* lat (void) { return &_latFact; } Fact* lon (void) { return &_lonFact; } + Fact* mgrs (void) { return &_mgrsFact; } Fact* hdop (void) { return &_hdopFact; } Fact* vdop (void) { return &_vdopFact; } Fact* courseOverGround (void) { return &_courseOverGroundFact; } @@ -233,6 +235,7 @@ public: static const char* _latFactName; static const char* _lonFactName; + static const char* _mgrsFactName; static const char* _hdopFactName; static const char* _vdopFactName; static const char* _courseOverGroundFactName; @@ -242,6 +245,7 @@ public: private: Fact _latFact; Fact _lonFact; + Fact _mgrsFact; Fact _hdopFact; Fact _vdopFact; Fact _courseOverGroundFact;