Skip to content

Q
qgroundcontrol
Commit 8e4fc06e authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add support for mavlink terrain frame

parent 308c8f2a
Hide whitespace changes
Inline Side-by-side
Showing with 98 additions and 38 deletions
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 8e4fc06e
......@@ -102,6 +102,7 @@ public:
QObject* loadParameterMetaData (const QString& metaDataFile) override;
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
bool supportsTerrainFrame (void) const override { return true; }
protected:
/// All access to singleton is through stack specific implementation
......
src/FirmwarePlugin/FirmwarePlugin.cc
View file @ 8e4fc06e
......@@ -150,6 +150,12 @@ bool FirmwarePlugin::supportsJSButton(void)
return false;
}
bool FirmwarePlugin::supportsTerrainFrame(void) const
{
// Generic firmware supports this since we don't know
return true;
}
bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
Q_UNUSED(vehicle);
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 8e4fc06e
......@@ -180,6 +180,9 @@ public:
/// (CompassMot). Default is true.
virtual bool supportsMotorInterference(void);
/// Returns true if the firmware supports MAV_FRAME_GLOBAL_TERRAIN_ALT
virtual bool supportsTerrainFrame(void) const;
/// Called before any mavlink message is processed by Vehicle such that the firmwre plugin
/// can adjust any message characteristics. This is handy to adjust or differences in mavlink
/// spec implementations such that the base code can remain mavlink generic.
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @ 8e4fc06e
......@@ -71,6 +71,7 @@ public:
QGCCameraManager* createCameraManager (Vehicle* vehicle) override;
QGCCameraControl* createCameraControl (const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL) override;
uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override;
bool supportsTerrainFrame (void) const override { return false; }
protected:
typedef struct {
......
src/MissionManager/SimpleMissionItem.cc
View file @ 8e4fc06e
......@@ -80,12 +80,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, QObject* parent)
setDefaultsForCommand();
_rebuildFacts();
connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged);
connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged);
connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirty);
connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber);
}
SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const MissionItem& missionItem, QObject* parent)
......@@ -98,7 +92,6 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss
, _cameraSection (NULL)
, _commandTree (qgcApp()->toolbox()->missionCommandTree())
, _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32)
, _altitudeMode (missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL)
, _altitudeFact (0, "Altitude", FactMetaData::valueTypeDouble)
, _amslAltAboveTerrainFact (0, "Alt above terrain", FactMetaData::valueTypeDouble)
, _param1MetaData (FactMetaData::valueTypeDouble)
......@@ -112,6 +105,25 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool editMode, const Miss
{
_editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml");
struct MavFrame2AltMode_s {
MAV_FRAME mavFrame;
AltitudeMode altMode;
};
const struct MavFrame2AltMode_s rgMavFrame2AltMode[] = {
{ MAV_FRAME_GLOBAL_TERRAIN_ALT, AltitudeTerrainFrame },
{ MAV_FRAME_GLOBAL, AltitudeAbsolute },
{ MAV_FRAME_GLOBAL_RELATIVE_ALT, AltitudeRelative },
};
_altitudeMode = AltitudeRelative;
for (size_t i=0; i<sizeof(rgMavFrame2AltMode)/sizeof(rgMavFrame2AltMode[0]); i++) {
const MavFrame2AltMode_s& pMavFrame2AltMode = rgMavFrame2AltMode[i];
if (pMavFrame2AltMode.mavFrame == missionItem.frame()) {
_altitudeMode = pMavFrame2AltMode.altMode;
break;
}
}
_isCurrentItem = missionItem.isCurrentItem();
_altitudeFact.setRawValue(specifiesCoordinate() ? _missionItem._param7Fact.rawValue() : qQNaN());
_amslAltAboveTerrainFact.setRawValue(qQNaN());
......@@ -176,12 +188,14 @@ void SimpleMissionItem::_connectSignals(void)
connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::_setDirty);
connect(this, &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_setDirty);
// These changes may need to trigger terrain queries
connect(&_altitudeFact, &Fact::valueChanged, this, &SimpleMissionItem::_altitudeChanged);
connect(this, &SimpleMissionItem::altitudeModeChanged,this, &SimpleMissionItem::_altitudeModeChanged);
connect(this, &SimpleMissionItem::terrainAltitudeChanged,this, &SimpleMissionItem::_terrainAltChanged);
connect(this, &SimpleMissionItem::sequenceNumberChanged, this, &SimpleMissionItem::lastSequenceNumberChanged);
connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_setDirty);
connect(this, &SimpleMissionItem::cameraSectionChanged, this, &SimpleMissionItem::_updateLastSequenceNumber);
// These are coordinate parameters, they must emit coordinateChanged signal
connect(&_missionItem._param5Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
connect(&_missionItem._param6Fact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCoordinateChanged);
......@@ -208,8 +222,12 @@ void SimpleMissionItem::_connectSignals(void)
// These fact signals must alway signal out through SimpleMissionItem signals
connect(&_missionItem._commandFact, &Fact::valueChanged, this, &SimpleMissionItem::_sendCommandChanged);
// Sequence number is kept in mission iteem, so we need to propagate signal up as well
connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged);
// Propogate signals from MissionItem up to SimpleMissionItem
connect(&_missionItem, &MissionItem::sequenceNumberChanged, this, &SimpleMissionItem::sequenceNumberChanged);
connect(&_missionItem, &MissionItem::specifiedFlightSpeedChanged, this, &SimpleMissionItem::specifiedFlightSpeedChanged);
// Firmware type change can affect supportsTerrainFrame
connect(_vehicle, &Vehicle::firmwareTypeChanged, this, &SimpleMissionItem::supportsTerrainFrameChanged);
}
void SimpleMissionItem::_setupMetaData(void)
......@@ -295,7 +313,7 @@ bool SimpleMissionItem::load(QTextStream &loadStream)
bool success;
if ((success = _missionItem.load(loadStream))) {
if (specifiesCoordinate()) {
_altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL;
_altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute;
_altitudeFact.setRawValue(_missionItem._param7Fact.rawValue());
_amslAltAboveTerrainFact.setRawValue(qQNaN());
}
......@@ -325,7 +343,7 @@ bool SimpleMissionItem::load(const QJsonObject& json, int sequenceNumber, QStrin
_altitudeFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey]));
_amslAltAboveTerrainFact.setRawValue(JsonHelper::possibleNaNJsonValue(json[_jsonAltitudeKey]));
} else {
_altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAMSL;
_altitudeMode = _missionItem.relativeAltitude() ? AltitudeRelative : AltitudeAbsolute;
_altitudeFact.setRawValue(_missionItem._param7Fact.rawValue());
_amslAltAboveTerrainFact.setRawValue(qQNaN());
}
......@@ -585,6 +603,8 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const
return true;
break;
case MAV_FRAME_GLOBAL_TERRAIN_ALT:
return supportsTerrainFrame();
default:
return false;
}
......@@ -636,21 +656,27 @@ void SimpleMissionItem::_sendCoordinateChanged(void)
void SimpleMissionItem::_altitudeModeChanged(void)
{
switch (_altitudeMode) {
case AltitudeTerrainFrame:
_missionItem.setFrame(MAV_FRAME_GLOBAL_TERRAIN_ALT);
break;
case AltitudeAboveTerrain:
// Terrain altitudes are AMSL
// Terrain altitudes are Absolute
_missionItem.setFrame(MAV_FRAME_GLOBAL);
// Clear any old values
// Clear any old calculated values
_missionItem._param7Fact.setRawValue(qQNaN());
_amslAltAboveTerrainFact.setRawValue(qQNaN());
_altitudeChanged();
break;
case AltitudeAMSL:
case AltitudeAbsolute:
_missionItem.setFrame(MAV_FRAME_GLOBAL);
break;
case AltitudeRelative:
_missionItem.setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
break;
}
// We always call _altitudeChanged to make sure that param7 is always setup correctly on mode change
_altitudeChanged();
emit coordinateHasRelativeAltitudeChanged(_altitudeMode == AltitudeRelative);
}
......
src/MissionManager/SimpleMissionItem.h
View file @ 8e4fc06e
......@@ -30,9 +30,10 @@ public:
~SimpleMissionItem();
enum AltitudeMode {
AltitudeRelative,
AltitudeAMSL,
AltitudeAboveTerrain
AltitudeRelative, // MAV_FRAME_GLOBAL_RELATIVE_ALT
AltitudeAbsolute, // MAV_FRAME_GLOBAL
AltitudeAboveTerrain, // Absolute altitude above terrain calculated from terrain data
AltitudeTerrainFrame // MAV_FRAME_GLOBAL_TERRAIN_ALT
};
Q_ENUM(AltitudeMode)
......@@ -45,6 +46,7 @@ public:
Q_PROPERTY(AltitudeMode altitudeMode READ altitudeMode WRITE setAltitudeMode NOTIFY altitudeModeChanged)
Q_PROPERTY(Fact* amslAltAboveTerrain READ amslAltAboveTerrain CONSTANT) ///< Actual AMSL altitude for item if altitudeMode == AltitudeAboveTerrain
Q_PROPERTY(int command READ command WRITE setCommand NOTIFY commandChanged)
Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY supportsTerrainFrameChanged)
/// Optional sections
Q_PROPERTY(QObject* speedSection READ speedSection NOTIFY speedSectionChanged)
......@@ -72,6 +74,7 @@ public:
AltitudeMode altitudeMode (void) const { return _altitudeMode; }
Fact* altitude (void) { return &_altitudeFact; }
Fact* amslAltAboveTerrain (void) { return &_amslAltAboveTerrainFact; }
bool supportsTerrainFrame(void) const { return _vehicle->supportsTerrainFrame(); }
CameraSection* cameraSection (void) { return _cameraSection; }
SpeedSection* speedSection (void) { return _speedSection; }
......@@ -141,6 +144,7 @@ signals:
void cameraSectionChanged (QObject* cameraSection);
void speedSectionChanged (QObject* cameraSection);
void altitudeModeChanged (void);
void supportsTerrainFrameChanged(void);
private slots:
void _setDirty (void);
......
src/MissionManager/SimpleMissionItemTest.cc
View file @ 8e4fc06e
......@@ -53,11 +53,11 @@ const SimpleMissionItemTest::FactValue_t SimpleMissionItemTest::_rgFactValuesDoJ
const SimpleMissionItemTest::ItemExpected_t SimpleMissionItemTest::_rgItemExpected[] = {
// Text field facts count Fact Values Altitude Altitude Mode
{ sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint)/sizeof(SimpleMissionItemTest::_rgFactValuesWaypoint[0]), SimpleMissionItemTest::_rgFactValuesWaypoint, 70.1234567, SimpleMissionItem::AltitudeRelative },
{ sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]), SimpleMissionItemTest::_rgFactValuesLoiterUnlim, 70.1234567, SimpleMissionItem::AltitudeAMSL },
{ sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterUnlim[0]), SimpleMissionItemTest::_rgFactValuesLoiterUnlim, 70.1234567, SimpleMissionItem::AltitudeAbsolute },
{ sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTurns[0]), SimpleMissionItemTest::_rgFactValuesLoiterTurns, 70.1234567, SimpleMissionItem::AltitudeRelative },
{ sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]), SimpleMissionItemTest::_rgFactValuesLoiterTime, 70.1234567, SimpleMissionItem::AltitudeAMSL },
{ sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime)/sizeof(SimpleMissionItemTest::_rgFactValuesLoiterTime[0]), SimpleMissionItemTest::_rgFactValuesLoiterTime, 70.1234567, SimpleMissionItem::AltitudeAbsolute },
{ 0, NULL, 70.1234567, SimpleMissionItem::AltitudeRelative },
{ sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]), SimpleMissionItemTest::_rgFactValuesTakeoff, 70.1234567, SimpleMissionItem::AltitudeAMSL },
{ sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff)/sizeof(SimpleMissionItemTest::_rgFactValuesTakeoff[0]), SimpleMissionItemTest::_rgFactValuesTakeoff, 70.1234567, SimpleMissionItem::AltitudeAbsolute },
{ sizeof(SimpleMissionItemTest::_rgFactValuesDoJump)/sizeof(SimpleMissionItemTest::_rgFactValuesDoJump[0]), SimpleMissionItemTest::_rgFactValuesDoJump, qQNaN(), SimpleMissionItem::AltitudeRelative },
};
......@@ -222,7 +222,7 @@ void SimpleMissionItemTest::_testSignals(void)
QVERIFY(_spyVisualItem->checkOnlySignalByMask(dirtyChangedMask));
_spyVisualItem->clearAllSignals();
_simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == SimpleMissionItem::AltitudeRelative ? SimpleMissionItem::AltitudeAMSL : SimpleMissionItem::AltitudeRelative);
_simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == SimpleMissionItem::AltitudeRelative ? SimpleMissionItem::AltitudeAbsolute : SimpleMissionItem::AltitudeRelative);
QVERIFY(_spySimpleItem->checkOnlySignalByMask(dirtyChangedMask | friendlyEditAllowedChangedMask | altitudeModeChangedMask | coordinateHasRelativeAltitudeChangedMask));
_spySimpleItem->clearAllSignals();
_spyVisualItem->clearAllSignals();
......@@ -314,7 +314,7 @@ void SimpleMissionItemTest::_testAltitudePropogation(void)
QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7());
QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL_RELATIVE_ALT);
_simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeAMSL);
_simpleItem->setAltitudeMode(SimpleMissionItem::AltitudeAbsolute);
_simpleItem->altitude()->setRawValue(_simpleItem->altitude()->rawValue().toDouble() + 1);
QCOMPARE(_simpleItem->altitude()->rawValue().toDouble(), _simpleItem->missionItem().param7());
QCOMPARE(_simpleItem->missionItem().frame(), MAV_FRAME_GLOBAL);
......
src/PlanView/SimpleItemEditor.qml
View file @ 8e4fc06e
......@@ -17,9 +17,14 @@ Rectangle {
color: qgcPal.windowShadeDark
radius: _radius
property bool _specifiesAltitude: missionItem.specifiesAltitude
property bool _altModeIsTerrain: missionItem.altitudeMode === 2
property real _margin: ScreenTools.defaultFontPixelHeight / 2
property bool _specifiesAltitude: missionItem.specifiesAltitude
property real _margin: ScreenTools.defaultFontPixelHeight / 2
property bool _supportsTerrainFrame: missionItem
readonly property int _altModeRelative: 0
readonly property int _altModeAbsolute: 1
readonly property int _altModeAboveTerrain: 2
readonly property int _altModeTerrainFrame: 3
ExclusiveGroup {
id: altRadios
......@@ -93,9 +98,10 @@ Rectangle {
}
RowLayout {
QGCRadioButton { text: qsTr("Rel"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 0 }
QGCRadioButton { text: qsTr("Abs"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 1 }
QGCRadioButton { text: qsTr("AGL"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: 2 }
QGCRadioButton { text: qsTr("Rel"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeRelative }
QGCRadioButton { text: qsTr("Abs"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeAbsolute }
QGCRadioButton { text: qsTr("AGL"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; readonly property int value: _altModeAboveTerrain }
QGCRadioButton { text: qsTr("TerrF"); exclusiveGroup: altRadios; checked: missionItem.altitudeMode === value; visible: missionItem.supportsTerrainFrame; readonly property int value: _altModeTerrainFrame }
}
FactValueSlider {
......@@ -107,17 +113,22 @@ Rectangle {
RowLayout {
spacing: _margin
visible: missionItem.altitudeMode === _altModeAboveTerrain
QGCLabel {
text: qsTr("Calculated Abs Alt")
font.pointSize: ScreenTools.smallFontPointSize
visible: _altModeIsTerrain
}
QGCLabel {
text: missionItem.amslAltAboveTerrain.valueString + " " + missionItem.amslAltAboveTerrain.units
visible: _altModeIsTerrain
}
}
QGCLabel {
text: qsTr("Using terrain reference frame")
font.pointSize: ScreenTools.smallFontPointSize
visible: missionItem.altitudeMode === _altModeTerrainFrame
}
}
}
......
src/Vehicle/Vehicle.cc
View file @ 8e4fc06e
......@@ -494,6 +494,7 @@ void Vehicle::prepareDelete()
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
_firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
_firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
emit firmwareTypeChanged();
if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
_capabilityBits = 0;
......@@ -2452,6 +2453,11 @@ bool Vehicle::supportsMotorInterference(void) const
return _firmwarePlugin->supportsMotorInterference();
}
bool Vehicle::supportsTerrainFrame(void) const
{
return _firmwarePlugin->supportsTerrainFrame();
}
QString Vehicle::vehicleTypeName() const {
static QMap<int, QString> typeNames = {
{ MAV_TYPE_GENERIC, tr("Generic micro air vehicle" )},
......
src/Vehicle/Vehicle.h
View file @ 8e4fc06e
......@@ -457,6 +457,7 @@ public:
Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged)
Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged)
Q_PROPERTY(bool highLatencyLink READ highLatencyLink NOTIFY highLatencyLinkChanged)
Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY firmwareTypeChanged)
// Vehicle state used for guided control
Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying
......@@ -665,11 +666,12 @@ public:
bool rover(void) const;
bool sub(void) const;
bool supportsThrottleModeCenterZero(void) const;
bool supportsNegativeThrust(void) const;
bool supportsRadio(void) const;
bool supportsJSButton(void) const;
bool supportsMotorInterference(void) const;
bool supportsThrottleModeCenterZero (void) const;
bool supportsNegativeThrust (void) const;
bool supportsRadio (void) const;
bool supportsJSButton (void) const;
bool supportsMotorInterference (void) const;
bool supportsTerrainFrame (void) const;
void setGuidedMode(bool guidedMode);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment