From a282a08b01567dc6fcd0639c0a781628d53a49aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 27 Jan 2021 10:20:14 +0100 Subject: [PATCH] Vehicle: add POSITION_TARGET_LOCAL_NED and LOCAL_POSITION_NED mavlink streams --- qgroundcontrol.pro | 4 ++ qgroundcontrol.qrc | 2 + src/Vehicle/LocalPositionFact.json | 49 ++++++++++++++++ src/Vehicle/Vehicle.cc | 8 +++ src/Vehicle/Vehicle.h | 10 ++++ src/Vehicle/VehicleLocalPositionFactGroup.cc | 65 ++++++++++++++++++++++ src/Vehicle/VehicleLocalPositionFactGroup.h | 53 ++++++++++++++++++ .../VehicleLocalPositionSetpointFactGroup.cc | 65 ++++++++++++++++++++++ .../VehicleLocalPositionSetpointFactGroup.h | 53 ++++++++++++++++++ 9 files changed, 309 insertions(+) create mode 100644 src/Vehicle/LocalPositionFact.json create mode 100644 src/Vehicle/VehicleLocalPositionFactGroup.cc create mode 100644 src/Vehicle/VehicleLocalPositionFactGroup.h create mode 100644 src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc create mode 100644 src/Vehicle/VehicleLocalPositionSetpointFactGroup.h 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; +};