diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9f6f937..ef55857 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -700,6 +700,7 @@ HEADERS += \ src/Vehicle/VehicleDistanceSensorFactGroup.h \ src/Vehicle/VehicleEstimatorStatusFactGroup.h \ src/Vehicle/VehicleGPSFactGroup.h \ + src/Vehicle/VehicleGPS2FactGroup.h \ src/Vehicle/VehicleLinkManager.h \ src/Vehicle/VehicleSetpointFactGroup.h \ src/Vehicle/VehicleTemperatureFactGroup.h \ @@ -930,6 +931,7 @@ SOURCES += \ src/Vehicle/VehicleDistanceSensorFactGroup.cc \ src/Vehicle/VehicleEstimatorStatusFactGroup.cc \ src/Vehicle/VehicleGPSFactGroup.cc \ + src/Vehicle/VehicleGPS2FactGroup.cc \ src/Vehicle/VehicleLinkManager.cc \ src/Vehicle/VehicleSetpointFactGroup.cc \ src/Vehicle/VehicleTemperatureFactGroup.cc \ diff --git a/src/Vehicle/CMakeLists.txt b/src/Vehicle/CMakeLists.txt index 6d1bcd3..9e70a07 100644 --- a/src/Vehicle/CMakeLists.txt +++ b/src/Vehicle/CMakeLists.txt @@ -57,6 +57,8 @@ add_library(Vehicle VehicleEstimatorStatusFactGroup.h VehicleGPSFactGroup.cc VehicleGPSFactGroup.h + VehicleGPS2FactGroup.cc + VehicleGPS2FactGroup.h Vehicle.h VehicleLinkManager.cc VehicleLinkManager.h diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 829d76c..3c3996f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -90,6 +90,7 @@ const char* Vehicle::_hobbsFactName = "hobbs"; const char* Vehicle::_throttlePctFactName = "throttlePct"; const char* Vehicle::_gpsFactGroupName = "gps"; +const char* Vehicle::_gps2FactGroupName = "gps2"; const char* Vehicle::_windFactGroupName = "wind"; const char* Vehicle::_vibrationFactGroupName = "vibration"; const char* Vehicle::_temperatureFactGroupName = "temperature"; @@ -143,6 +144,7 @@ Vehicle::Vehicle(LinkInterface* link, , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) , _gpsFactGroup (this) + , _gps2FactGroup (this) , _windFactGroup (this) , _vibrationFactGroup (this) , _temperatureFactGroup (this) @@ -289,6 +291,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) , _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16) , _gpsFactGroup (this) + , _gps2FactGroup (this) , _windFactGroup (this) , _vibrationFactGroup (this) , _clockFactGroup (this) @@ -398,6 +401,7 @@ void Vehicle::_commonInit() _addFact(&_hobbsFact, _hobbsFactName); _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); + _addFactGroup(&_gps2FactGroup, _gps2FactGroupName); _addFactGroup(&_windFactGroup, _windFactGroupName); _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); @@ -662,6 +666,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_GPS_RAW_INT: _handleGpsRawInt(message); break; + case MAVLINK_MSG_ID_GPS2_RAW: + _handleGps2Raw(message); + break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: _handleGlobalPositionInt(message); break; @@ -1025,6 +1032,27 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) } } +void Vehicle::_handleGps2Raw(mavlink_message_t& message) +{ + mavlink_gps2_raw_t gpsRaw; + mavlink_msg_gps2_raw_decode(&message, &gpsRaw); + + _gps2RawMessageAvailable = true; + + if (gpsRaw.fix_type >= GPS_FIX_TYPE_3D_FIX) { + if (!_globalPositionIntMessageAvailable) { + QGeoCoordinate newPosition(gpsRaw.lat / (double)1E7, gpsRaw.lon / (double)1E7, gpsRaw.alt / 1000.0); + if (newPosition != _coordinate) { + _coordinate = newPosition; + emit coordinateChanged(_coordinate); + } + if (!_altitudeMessageAvailable) { + _altitudeAMSLFact.setRawValue(gpsRaw.alt / 1000.0); + } + } + } +} + void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) { mavlink_global_position_int_t globalPositionInt; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 0257fe1..356a671 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -29,6 +29,7 @@ #include "VehicleDistanceSensorFactGroup.h" #include "VehicleWindFactGroup.h" #include "VehicleGPSFactGroup.h" +#include "VehicleGPS2FactGroup.h" #include "VehicleSetpointFactGroup.h" #include "VehicleTemperatureFactGroup.h" #include "VehicleVibrationFactGroup.h" @@ -280,6 +281,7 @@ public: Q_PROPERTY(Fact* throttlePct READ throttlePct CONSTANT) Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT) + Q_PROPERTY(FactGroup* gps2 READ gps2FactGroup CONSTANT) Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT) Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) @@ -601,6 +603,7 @@ public: Fact* throttlePct () { return &_throttlePctFact; } FactGroup* gpsFactGroup () { return &_gpsFactGroup; } + FactGroup* gps2FactGroup () { return &_gps2FactGroup; } FactGroup* windFactGroup () { return &_windFactGroup; } FactGroup* vibrationFactGroup () { return &_vibrationFactGroup; } FactGroup* temperatureFactGroup () { return &_temperatureFactGroup; } @@ -907,6 +910,7 @@ private: void _handleExtendedSysState (mavlink_message_t& message); void _handleCommandAck (mavlink_message_t& message); void _handleGpsRawInt (mavlink_message_t& message); + void _handleGps2Raw (mavlink_message_t& message); void _handleGlobalPositionInt (mavlink_message_t& message); void _handleAltitude (mavlink_message_t& message); void _handleVfrHud (mavlink_message_t& message); @@ -993,6 +997,7 @@ private: uint32_t _onboardControlSensorsHealth = 0; uint32_t _onboardControlSensorsUnhealthy = 0; bool _gpsRawIntMessageAvailable = false; + bool _gps2RawMessageAvailable = false; bool _globalPositionIntMessageAvailable = false; bool _altitudeMessageAvailable = false; double _defaultCruiseSpeed = qQNaN(); @@ -1206,6 +1211,7 @@ private: Fact _throttlePctFact; VehicleGPSFactGroup _gpsFactGroup; + VehicleGPS2FactGroup _gps2FactGroup; VehicleWindFactGroup _windFactGroup; VehicleVibrationFactGroup _vibrationFactGroup; VehicleTemperatureFactGroup _temperatureFactGroup; @@ -1248,6 +1254,7 @@ private: static const char* _throttlePctFactName; static const char* _gpsFactGroupName; + static const char* _gps2FactGroupName; static const char* _windFactGroupName; static const char* _vibrationFactGroupName; static const char* _temperatureFactGroupName; diff --git a/src/Vehicle/VehicleGPS2FactGroup.cc b/src/Vehicle/VehicleGPS2FactGroup.cc new file mode 100644 index 0000000..c88d70f --- /dev/null +++ b/src/Vehicle/VehicleGPS2FactGroup.cc @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleGPS2FactGroup.h" +#include "Vehicle.h" +#include "QGCGeo.h" + +VehicleGPS2FactGroup::VehicleGPS2FactGroup(QObject* parent) + : VehicleGPSFactGroup(parent) {} + +void VehicleGPS2FactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_GPS2_RAW: + _handleGps2Raw(message); + break; + default: + break; + } +} + +void VehicleGPS2FactGroup::_handleGps2Raw(mavlink_message_t& message) +{ + mavlink_gps2_raw_t gps2Raw; + mavlink_msg_gps2_raw_decode(&message, &gps2Raw); + + lat()->setRawValue (gps2Raw.lat * 1e-7); + lon()->setRawValue (gps2Raw.lon * 1e-7); + mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(gps2Raw.lat * 1e-7, gps2Raw.lon * 1e-7))); + count()->setRawValue (gps2Raw.satellites_visible == 255 ? 0 : gps2Raw.satellites_visible); + hdop()->setRawValue (gps2Raw.eph == UINT16_MAX ? qQNaN() : gps2Raw.eph / 100.0); + vdop()->setRawValue (gps2Raw.epv == UINT16_MAX ? qQNaN() : gps2Raw.epv / 100.0); + courseOverGround()->setRawValue (gps2Raw.cog == UINT16_MAX ? qQNaN() : gps2Raw.cog / 100.0); + lock()->setRawValue (gps2Raw.fix_type); +} \ No newline at end of file diff --git a/src/Vehicle/VehicleGPS2FactGroup.h b/src/Vehicle/VehicleGPS2FactGroup.h new file mode 100644 index 0000000..9fc48ef --- /dev/null +++ b/src/Vehicle/VehicleGPS2FactGroup.h @@ -0,0 +1,27 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "VehicleGPSFactGroup.h" +#include "QGCMAVLink.h" + +class VehicleGPS2FactGroup : public VehicleGPSFactGroup +{ + Q_OBJECT + +public: + VehicleGPS2FactGroup(QObject* parent = nullptr); + + // Overrides from VehicleGPSFactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + +private: + void _handleGps2Raw(mavlink_message_t& message); +}; diff --git a/src/Vehicle/VehicleGPSFactGroup.h b/src/Vehicle/VehicleGPSFactGroup.h index a08fd09..505717c 100644 --- a/src/Vehicle/VehicleGPSFactGroup.h +++ b/src/Vehicle/VehicleGPSFactGroup.h @@ -38,7 +38,7 @@ public: Fact* lock () { return &_lockFact; } // Overrides from FactGroup - void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; static const char* _latFactName; static const char* _lonFactName; @@ -49,7 +49,7 @@ public: static const char* _countFactName; static const char* _lockFactName; -private: +protected: void _handleGpsRawInt (mavlink_message_t& message); void _handleHighLatency (mavlink_message_t& message); void _handleHighLatency2(mavlink_message_t& message);