diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 241e255..a0f6230 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -700,6 +700,8 @@ HEADERS += \
src/Vehicle/VehicleClockFactGroup.h \
src/Vehicle/VehicleDistanceSensorFactGroup.h \
src/Vehicle/VehicleEstimatorStatusFactGroup.h \
+ src/Vehicle/VehicleLocalPositionFactGroup.h \
+ src/Vehicle/VehicleLocalPositionSetpointFactGroup.h \
src/Vehicle/VehicleGPSFactGroup.h \
src/Vehicle/VehicleGPS2FactGroup.h \
src/Vehicle/VehicleLinkManager.h \
@@ -932,6 +934,8 @@ SOURCES += \
src/Vehicle/VehicleClockFactGroup.cc \
src/Vehicle/VehicleDistanceSensorFactGroup.cc \
src/Vehicle/VehicleEstimatorStatusFactGroup.cc \
+ src/Vehicle/VehicleLocalPositionFactGroup.cc \
+ src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc \
src/Vehicle/VehicleGPSFactGroup.cc \
src/Vehicle/VehicleGPS2FactGroup.cc \
src/Vehicle/VehicleLinkManager.cc \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 5ca77ef..3682da7 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -323,6 +323,8 @@
src/Vehicle/GPSFact.json
src/Vehicle/GPSRTKFact.json
src/Vehicle/SetpointFact.json
+ src/Vehicle/LocalPositionFact.json
+ src/Vehicle/LocalPositionFact.json
src/Vehicle/SubmarineFact.json
src/Vehicle/TemperatureFact.json
src/Vehicle/TerrainFactGroup.json
diff --git a/src/Vehicle/LocalPositionFact.json b/src/Vehicle/LocalPositionFact.json
new file mode 100644
index 0000000..61c7dc3
--- /dev/null
+++ b/src/Vehicle/LocalPositionFact.json
@@ -0,0 +1,49 @@
+{
+ "version": 1,
+ "fileType": "FactMetaData",
+ "QGC.MetaData.Facts":
+[
+{
+ "name": "x",
+ "shortDesc": "X",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m"
+},
+{
+ "name": "y",
+ "shortDesc": "Y",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m"
+},
+{
+ "name": "z",
+ "shortDesc": "Z",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m"
+},
+{
+ "name": "vx",
+ "shortDesc": "VX",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m/s"
+},
+{
+ "name": "vy",
+ "shortDesc": "Vy",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m/s"
+},
+{
+ "name": "vz",
+ "shortDesc": "Vz",
+ "type": "double",
+ "decimalPlaces": 1,
+ "units": "m/s"
+}
+]
+}
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 5bf1dce..9bc045f 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -97,6 +97,8 @@ const char* Vehicle::_temperatureFactGroupName = "temperature";
const char* Vehicle::_clockFactGroupName = "clock";
const char* Vehicle::_setpointFactGroupName = "setpoint";
const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor";
+const char* Vehicle::_localPositionFactGroupName = "localPosition";
+const char* Vehicle::_localPositionSetpointFactGroupName ="localPositionSetpoint";
const char* Vehicle::_escStatusFactGroupName = "escStatus";
const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus";
const char* Vehicle::_terrainFactGroupName = "terrain";
@@ -150,6 +152,8 @@ Vehicle::Vehicle(LinkInterface* link,
, _clockFactGroup (this)
, _setpointFactGroup (this)
, _distanceSensorFactGroup (this)
+ , _localPositionFactGroup (this)
+ , _localPositionSetpointFactGroup(this)
, _escStatusFactGroup (this)
, _estimatorStatusFactGroup (this)
, _terrainFactGroup (this)
@@ -296,6 +300,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _vibrationFactGroup (this)
, _clockFactGroup (this)
, _distanceSensorFactGroup (this)
+ , _localPositionFactGroup (this)
+ , _localPositionSetpointFactGroup (this)
{
_linkManager = _toolbox->linkManager();
@@ -408,6 +414,8 @@ void Vehicle::_commonInit()
_addFactGroup(&_clockFactGroup, _clockFactGroupName);
_addFactGroup(&_setpointFactGroup, _setpointFactGroupName);
_addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName);
+ _addFactGroup(&_localPositionFactGroup, _localPositionFactGroupName);
+ _addFactGroup(&_localPositionSetpointFactGroup,_localPositionSetpointFactGroupName);
_addFactGroup(&_escStatusFactGroup, _escStatusFactGroupName);
_addFactGroup(&_estimatorStatusFactGroup, _estimatorStatusFactGroupName);
_addFactGroup(&_terrainFactGroup, _terrainFactGroupName);
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 5b7a703..9564f7a 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -28,6 +28,8 @@
#include "SysStatusSensorInfo.h"
#include "VehicleClockFactGroup.h"
#include "VehicleDistanceSensorFactGroup.h"
+#include "VehicleLocalPositionFactGroup.h"
+#include "VehicleLocalPositionSetpointFactGroup.h"
#include "VehicleWindFactGroup.h"
#include "VehicleGPSFactGroup.h"
#include "VehicleGPS2FactGroup.h"
@@ -293,6 +295,8 @@ public:
Q_PROPERTY(FactGroup* estimatorStatus READ estimatorStatusFactGroup CONSTANT)
Q_PROPERTY(FactGroup* terrain READ terrainFactGroup CONSTANT)
Q_PROPERTY(FactGroup* distanceSensors READ distanceSensorFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* localPosition READ localPositionFactGroup CONSTANT)
+ Q_PROPERTY(FactGroup* localPositionSetpoint READ localPositionSetpointFactGroup CONSTANT)
Q_PROPERTY(QmlObjectListModel* batteries READ batteries CONSTANT)
Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged)
@@ -614,6 +618,8 @@ public:
FactGroup* clockFactGroup () { return &_clockFactGroup; }
FactGroup* setpointFactGroup () { return &_setpointFactGroup; }
FactGroup* distanceSensorFactGroup () { return &_distanceSensorFactGroup; }
+ FactGroup* localPositionFactGroup () { return &_localPositionFactGroup; }
+ FactGroup* localPositionSetpointFactGroup() { return &_localPositionSetpointFactGroup; }
FactGroup* escStatusFactGroup () { return &_escStatusFactGroup; }
FactGroup* estimatorStatusFactGroup () { return &_estimatorStatusFactGroup; }
FactGroup* terrainFactGroup () { return &_terrainFactGroup; }
@@ -1216,6 +1222,8 @@ private:
VehicleClockFactGroup _clockFactGroup;
VehicleSetpointFactGroup _setpointFactGroup;
VehicleDistanceSensorFactGroup _distanceSensorFactGroup;
+ VehicleLocalPositionFactGroup _localPositionFactGroup;
+ VehicleLocalPositionSetpointFactGroup _localPositionSetpointFactGroup;
VehicleEscStatusFactGroup _escStatusFactGroup;
VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup;
TerrainFactGroup _terrainFactGroup;
@@ -1259,6 +1267,8 @@ private:
static const char* _clockFactGroupName;
static const char* _setpointFactGroupName;
static const char* _distanceSensorFactGroupName;
+ static const char* _localPositionFactGroupName;
+ static const char* _localPositionSetpointFactGroupName;
static const char* _escStatusFactGroupName;
static const char* _estimatorStatusFactGroupName;
static const char* _terrainFactGroupName;
diff --git a/src/Vehicle/VehicleLocalPositionFactGroup.cc b/src/Vehicle/VehicleLocalPositionFactGroup.cc
new file mode 100644
index 0000000..deb0d85
--- /dev/null
+++ b/src/Vehicle/VehicleLocalPositionFactGroup.cc
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * (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 "VehicleLocalPositionFactGroup.h"
+#include "Vehicle.h"
+
+#include
+
+const char* VehicleLocalPositionFactGroup::_xFactName = "x";
+const char* VehicleLocalPositionFactGroup::_yFactName = "y";
+const char* VehicleLocalPositionFactGroup::_zFactName = "z";
+const char* VehicleLocalPositionFactGroup::_vxFactName = "vx";
+const char* VehicleLocalPositionFactGroup::_vyFactName = "vy";
+const char* VehicleLocalPositionFactGroup::_vzFactName = "vz";
+
+VehicleLocalPositionFactGroup::VehicleLocalPositionFactGroup(QObject* parent)
+ : FactGroup (1000, ":/json/Vehicle/LocalPositionFact.json", parent)
+ , _xFact (0, _xFactName, FactMetaData::valueTypeDouble)
+ , _yFact (0, _yFactName, FactMetaData::valueTypeDouble)
+ , _zFact (0, _zFactName, FactMetaData::valueTypeDouble)
+ , _vxFact (0, _vxFactName, FactMetaData::valueTypeDouble)
+ , _vyFact (0, _vyFactName, FactMetaData::valueTypeDouble)
+ , _vzFact (0, _vzFactName, FactMetaData::valueTypeDouble)
+{
+ _addFact(&_xFact, _xFactName);
+ _addFact(&_yFact, _yFactName);
+ _addFact(&_zFact, _zFactName);
+ _addFact(&_vxFact, _vxFactName);
+ _addFact(&_vyFact, _vyFactName);
+ _addFact(&_vzFact, _vzFactName);
+
+ // Start out as not available "--.--"
+ _xFact.setRawValue(qQNaN());
+ _yFact.setRawValue(qQNaN());
+ _zFact.setRawValue(qQNaN());
+ _vxFact.setRawValue(qQNaN());
+ _vyFact.setRawValue(qQNaN());
+ _vzFact.setRawValue(qQNaN());
+}
+
+void VehicleLocalPositionFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
+{
+ if (message.msgid != MAVLINK_MSG_ID_LOCAL_POSITION_NED) {
+ return;
+ }
+
+ mavlink_local_position_ned_t localPosition;
+ mavlink_msg_local_position_ned_decode(&message, &localPosition);
+
+ x()->setRawValue(localPosition.x);
+ y()->setRawValue(localPosition.y);
+ z()->setRawValue(localPosition.z);
+
+ vx()->setRawValue(localPosition.vx);
+ vy()->setRawValue(localPosition.vy);
+ vz()->setRawValue(localPosition.vz);
+
+ _setTelemetryAvailable(true);
+}
diff --git a/src/Vehicle/VehicleLocalPositionFactGroup.h b/src/Vehicle/VehicleLocalPositionFactGroup.h
new file mode 100644
index 0000000..f3916b8
--- /dev/null
+++ b/src/Vehicle/VehicleLocalPositionFactGroup.h
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ * (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 "FactGroup.h"
+#include "QGCMAVLink.h"
+
+class VehicleLocalPositionFactGroup : public FactGroup
+{
+ Q_OBJECT
+
+public:
+ VehicleLocalPositionFactGroup(QObject* parent = nullptr);
+
+ Q_PROPERTY(Fact* x READ x CONSTANT)
+ Q_PROPERTY(Fact* y READ y CONSTANT)
+ Q_PROPERTY(Fact* z READ z CONSTANT)
+ Q_PROPERTY(Fact* vx READ vx CONSTANT)
+ Q_PROPERTY(Fact* vy READ vy CONSTANT)
+ Q_PROPERTY(Fact* vz READ vz CONSTANT)
+
+ Fact* x () { return &_xFact; }
+ Fact* y () { return &_yFact; }
+ Fact* z () { return &_zFact; }
+ Fact* vx () { return &_vxFact; }
+ Fact* vy () { return &_vyFact; }
+ Fact* vz () { return &_vzFact; }
+
+ // Overrides from FactGroup
+ void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
+
+ static const char* _xFactName;
+ static const char* _yFactName;
+ static const char* _zFactName;
+ static const char* _vxFactName;
+ static const char* _vyFactName;
+ static const char* _vzFactName;
+
+private:
+ Fact _xFact;
+ Fact _yFact;
+ Fact _zFact;
+ Fact _vxFact;
+ Fact _vyFact;
+ Fact _vzFact;
+};
diff --git a/src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc b/src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc
new file mode 100644
index 0000000..0df423f
--- /dev/null
+++ b/src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * (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 "VehicleLocalPositionSetpointFactGroup.h"
+#include "Vehicle.h"
+
+#include
+
+const char* VehicleLocalPositionSetpointFactGroup::_xFactName = "x";
+const char* VehicleLocalPositionSetpointFactGroup::_yFactName = "y";
+const char* VehicleLocalPositionSetpointFactGroup::_zFactName = "z";
+const char* VehicleLocalPositionSetpointFactGroup::_vxFactName = "vx";
+const char* VehicleLocalPositionSetpointFactGroup::_vyFactName = "vy";
+const char* VehicleLocalPositionSetpointFactGroup::_vzFactName = "vz";
+
+VehicleLocalPositionSetpointFactGroup::VehicleLocalPositionSetpointFactGroup(QObject* parent)
+ : FactGroup (1000, ":/json/Vehicle/LocalPositionSetpointFact.json", parent)
+ , _xFact (0, _xFactName, FactMetaData::valueTypeDouble)
+ , _yFact (0, _yFactName, FactMetaData::valueTypeDouble)
+ , _zFact (0, _zFactName, FactMetaData::valueTypeDouble)
+ , _vxFact (0, _vxFactName, FactMetaData::valueTypeDouble)
+ , _vyFact (0, _vyFactName, FactMetaData::valueTypeDouble)
+ , _vzFact (0, _vzFactName, FactMetaData::valueTypeDouble)
+{
+ _addFact(&_xFact, _xFactName);
+ _addFact(&_yFact, _yFactName);
+ _addFact(&_zFact, _zFactName);
+ _addFact(&_vxFact, _vxFactName);
+ _addFact(&_vyFact, _vyFactName);
+ _addFact(&_vzFact, _vzFactName);
+
+ // Start out as not available "--.--"
+ _xFact.setRawValue(qQNaN());
+ _yFact.setRawValue(qQNaN());
+ _zFact.setRawValue(qQNaN());
+ _vxFact.setRawValue(qQNaN());
+ _vyFact.setRawValue(qQNaN());
+ _vzFact.setRawValue(qQNaN());
+}
+
+void VehicleLocalPositionSetpointFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
+{
+ if (message.msgid != MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED) {
+ return;
+ }
+
+ mavlink_position_target_local_ned_t localPosition;
+ mavlink_msg_position_target_local_ned_decode(&message, &localPosition);
+
+ x()->setRawValue(localPosition.x);
+ y()->setRawValue(localPosition.y);
+ z()->setRawValue(localPosition.z);
+
+ vx()->setRawValue(localPosition.vx);
+ vy()->setRawValue(localPosition.vy);
+ vz()->setRawValue(localPosition.vz);
+
+ _setTelemetryAvailable(true);
+}
diff --git a/src/Vehicle/VehicleLocalPositionSetpointFactGroup.h b/src/Vehicle/VehicleLocalPositionSetpointFactGroup.h
new file mode 100644
index 0000000..164bab5
--- /dev/null
+++ b/src/Vehicle/VehicleLocalPositionSetpointFactGroup.h
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ * (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 "FactGroup.h"
+#include "QGCMAVLink.h"
+
+class VehicleLocalPositionSetpointFactGroup : public FactGroup
+{
+ Q_OBJECT
+
+public:
+ VehicleLocalPositionSetpointFactGroup(QObject* parent = nullptr);
+
+ Q_PROPERTY(Fact* x READ x CONSTANT)
+ Q_PROPERTY(Fact* y READ y CONSTANT)
+ Q_PROPERTY(Fact* z READ z CONSTANT)
+ Q_PROPERTY(Fact* vx READ vx CONSTANT)
+ Q_PROPERTY(Fact* vy READ vy CONSTANT)
+ Q_PROPERTY(Fact* vz READ vz CONSTANT)
+
+ Fact* x () { return &_xFact; }
+ Fact* y () { return &_yFact; }
+ Fact* z () { return &_zFact; }
+ Fact* vx () { return &_vxFact; }
+ Fact* vy () { return &_vyFact; }
+ Fact* vz () { return &_vzFact; }
+
+ // Overrides from FactGroup
+ void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
+
+ static const char* _xFactName;
+ static const char* _yFactName;
+ static const char* _zFactName;
+ static const char* _vxFactName;
+ static const char* _vyFactName;
+ static const char* _vzFactName;
+
+private:
+ Fact _xFact;
+ Fact _yFact;
+ Fact _zFact;
+ Fact _vxFact;
+ Fact _vyFact;
+ Fact _vzFact;
+};