From 72e5769b1c55f7e96ab13d0be805de288c7cce56 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 25 Jul 2017 13:07:47 -0400 Subject: [PATCH] Add gps coordinates to vehicle gps factgroup --- src/Vehicle/GPSFact.json | 12 ++++++++++++ src/Vehicle/Vehicle.cc | 10 ++++++++++ src/Vehicle/Vehicle.h | 8 ++++++++ 3 files changed, 30 insertions(+) diff --git a/src/Vehicle/GPSFact.json b/src/Vehicle/GPSFact.json index 9b018b459..d9b3c04af 100644 --- a/src/Vehicle/GPSFact.json +++ b/src/Vehicle/GPSFact.json @@ -1,4 +1,16 @@ [ +{ + "name": "lat", + "shortDescription": "Latitude", + "type": "double", + "decimalPlaces": 7 +}, +{ + "name": "lon", + "shortDescription": "Longitude", + "type": "double", + "decimalPlaces": 7 +}, { "name": "hdop", "shortDescription": "HDOP", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 450629425..b813c0aa8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -680,6 +680,8 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) } } + _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7); + _gpsFactGroup.lon()->setRawValue(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); @@ -2458,6 +2460,8 @@ void Vehicle::triggerCamera(void) 1.0); // test shot flag } +const char* VehicleGPSFactGroup::_latFactName = "lat"; +const char* VehicleGPSFactGroup::_lonFactName = "lon"; const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; @@ -2466,18 +2470,24 @@ const char* VehicleGPSFactGroup::_lockFactName = "lock"; VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent) + , _latFact (0, _latFactName, FactMetaData::valueTypeDouble) + , _lonFact (0, _lonFactName, FactMetaData::valueTypeDouble) , _hdopFact (0, _hdopFactName, FactMetaData::valueTypeDouble) , _vdopFact (0, _vdopFactName, FactMetaData::valueTypeDouble) , _courseOverGroundFact (0, _courseOverGroundFactName, FactMetaData::valueTypeDouble) , _countFact (0, _countFactName, FactMetaData::valueTypeInt32) , _lockFact (0, _lockFactName, FactMetaData::valueTypeInt32) { + _addFact(&_latFact, _latFactName); + _addFact(&_lonFact, _lonFactName); _addFact(&_hdopFact, _hdopFactName); _addFact(&_vdopFact, _vdopFactName); _addFact(&_courseOverGroundFact, _courseOverGroundFactName); _addFact(&_lockFact, _lockFactName); _addFact(&_countFact, _countFactName); + _latFact.setRawValue(std::numeric_limits::quiet_NaN()); + _lonFact.setRawValue(std::numeric_limits::quiet_NaN()); _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 c099b88ed..dd524bcd3 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -113,18 +113,24 @@ class VehicleGPSFactGroup : public FactGroup public: VehicleGPSFactGroup(QObject* parent = NULL); + Q_PROPERTY(Fact* lat READ lat CONSTANT) + Q_PROPERTY(Fact* lon READ lon CONSTANT) Q_PROPERTY(Fact* hdop READ hdop CONSTANT) Q_PROPERTY(Fact* vdop READ vdop CONSTANT) Q_PROPERTY(Fact* courseOverGround READ courseOverGround CONSTANT) Q_PROPERTY(Fact* count READ count CONSTANT) Q_PROPERTY(Fact* lock READ lock CONSTANT) + Fact* lat (void) { return &_latFact; } + Fact* lon (void) { return &_lonFact; } Fact* hdop (void) { return &_hdopFact; } Fact* vdop (void) { return &_vdopFact; } Fact* courseOverGround (void) { return &_courseOverGroundFact; } Fact* count (void) { return &_countFact; } Fact* lock (void) { return &_lockFact; } + static const char* _latFactName; + static const char* _lonFactName; static const char* _hdopFactName; static const char* _vdopFactName; static const char* _courseOverGroundFactName; @@ -132,6 +138,8 @@ public: static const char* _lockFactName; private: + Fact _latFact; + Fact _lonFact; Fact _hdopFact; Fact _vdopFact; Fact _courseOverGroundFact; -- 2.22.0