zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Vehicle: add POSITION_TARGET_LOCAL_NED and LOCAL_POSITION_NED mavlink streams

QGC4.4
Beat Küng 4 years ago committed by Lorenz Meier
parent
30cff8084f
commit
a282a08b01
9 changed files with 309 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      qgroundcontrol.pro
  2. 2
      qgroundcontrol.qrc
  3. 49
      src/Vehicle/LocalPositionFact.json
  4. 8
      src/Vehicle/Vehicle.cc
  5. 10
      src/Vehicle/Vehicle.h
  6. 65
      src/Vehicle/VehicleLocalPositionFactGroup.cc
  7. 53
      src/Vehicle/VehicleLocalPositionFactGroup.h
  8. 65
      src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc
  9. 53
      src/Vehicle/VehicleLocalPositionSetpointFactGroup.h

4
qgroundcontrol.pro
Unescape View File

@ -700,6 +700,8 @@ HEADERS += \ @@ -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 += \ @@ -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 \

2
qgroundcontrol.qrc
Unescape View File

@ -323,6 +323,8 @@ @@ -323,6 +323,8 @@
<file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file>
<file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>
<file alias="Vehicle/LocalPositionFact.json">src/Vehicle/LocalPositionFact.json</file>
<file alias="Vehicle/LocalPositionSetpointFact.json">src/Vehicle/LocalPositionFact.json</file>
<file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file>
<file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file>
<file alias="Vehicle/TerrainFactGroup.json">src/Vehicle/TerrainFactGroup.json</file>

49
src/Vehicle/LocalPositionFact.json
Unescape View File

@ -0,0 +1,49 @@ @@ -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"
}
]
}

8
src/Vehicle/Vehicle.cc
Unescape View File

@ -97,6 +97,8 @@ const char* Vehicle::_temperatureFactGroupName = "temperature"; @@ -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, @@ -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, @@ -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() @@ -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);

10
src/Vehicle/Vehicle.h
Unescape View File

@ -28,6 +28,8 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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;

65
src/Vehicle/VehicleLocalPositionFactGroup.cc
Unescape View File

@ -0,0 +1,65 @@ @@ -0,0 +1,65 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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 <QtMath>
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);
}

53
src/Vehicle/VehicleLocalPositionFactGroup.h
Unescape View File

@ -0,0 +1,53 @@ @@ -0,0 +1,53 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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;
};

65
src/Vehicle/VehicleLocalPositionSetpointFactGroup.cc
Unescape View File

@ -0,0 +1,65 @@ @@ -0,0 +1,65 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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 <QtMath>
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);
}

53
src/Vehicle/VehicleLocalPositionSetpointFactGroup.h
Unescape View File

@ -0,0 +1,53 @@ @@ -0,0 +1,53 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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;
};
Write Preview
Loading…
Cancel
Save