Skip to content

Q
qgroundcontrol
Unverified Commit 48ffa39e authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #9071 from DonLakeFlyer/LandingPattern 

Landing Pattern: Loiter to alt optional support
parents a671b146 138bdfa9
Hide whitespace changes
Inline Side-by-side
Showing with 1549 additions and 1357 deletions
qgroundcontrol.pro
View file @ 48ffa39e
......@@ -472,6 +472,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/MissionManager/CameraSectionTest.h \
src/MissionManager/CorridorScanComplexItemTest.h \
src/MissionManager/FWLandingPatternTest.h \
src/MissionManager/LandingComplexItemTest.h \
src/MissionManager/MissionCommandTreeEditorTest.h \
src/MissionManager/MissionCommandTreeTest.h \
src/MissionManager/MissionControllerManagerTest.h \
......@@ -519,6 +520,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
src/MissionManager/CameraSectionTest.cc \
src/MissionManager/CorridorScanComplexItemTest.cc \
src/MissionManager/FWLandingPatternTest.cc \
src/MissionManager/LandingComplexItemTest.cc \
src/MissionManager/MissionCommandTreeEditorTest.cc \
src/MissionManager/MissionCommandTreeTest.cc \
src/MissionManager/MissionControllerManagerTest.cc \
......
src/MissionManager/CameraSection.h
View file @ 48ffa39e
......@@ -17,6 +17,8 @@
#define VIDEO_CAPTURE_STATUS_INTERVAL 0.2 //-- Send capture status every 5 seconds
class PlanMasterController;
class CameraSectionTest;
class CameraSection : public Section
{
......@@ -131,4 +133,6 @@ private:
static const char* _cameraPhotoIntervalDistanceName;
static const char* _cameraPhotoIntervalTimeName;
static const char* _cameraModeName;
friend CameraSectionTest;
};
src/MissionManager/CameraSectionTest.cc
View file @ 48ffa39e
......@@ -12,6 +12,8 @@
#include "MissionCommandTree.h"
#include "MissionCommandUIInfo.h"
#include <functional>
CameraSectionTest::CameraSectionTest(void)
: _spyCamera (nullptr)
, _spySection (nullptr)
......@@ -44,11 +46,24 @@ void CameraSectionTest::init(void)
_validGimbalItem = new SimpleMissionItem(_masterController,
false, // flyView
MissionItem(0, MAV_CMD_DO_MOUNT_CONTROL, MAV_FRAME_MISSION, 10.1234, 0, 20.1234, 0, 0, 0, MAV_MOUNT_MODE_MAVLINK_TARGETING, true, false),
MissionItem(0,
MAV_CMD_DO_MOUNT_CONTROL,
MAV_FRAME_MISSION,
10.1234, 0, 20.1234, // pitch, roll, yaw
0, 0, 0, // alt, lat, lon (all 0 since unused)
MAV_MOUNT_MODE_MAVLINK_TARGETING, // control gimbal with pitch, roll, yaw settings
true, false),
this);
_validTimeItem = new SimpleMissionItem(_masterController,
false, // flyView
MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, 0, 48, 0, NAN, NAN, NAN, NAN, true, false),
MissionItem(0,
MAV_CMD_IMAGE_START_CAPTURE,
MAV_FRAME_MISSION,
0, // Reserved, must be 0
48, // time interval
0, // 0 = capture forever
NAN, NAN, NAN, NAN, // Reserved
true, false),
this);
_validDistanceItem = new SimpleMissionItem(_masterController,
false, // flyView
......@@ -615,12 +630,12 @@ void CameraSectionTest::_testScanForGimbalSection(void)
Mission Param #5 WIP: latitude in degrees * 1E7, set if appropriate mount mode.
Mission Param #6 WIP: longitude in degrees * 1E7, set if appropriate mount mode.
Mission Param #7 MAV_MOUNT_MODE enum value
*/
*/
// Gimbal command but incorrect settings
SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validGimbalItem->missionItem(), nullptr);
invalidSimpleItem.missionItem().setParam2(10); // roll is not supported
invalidSimpleItem.missionItem().setParam2(10); // roll is not supported, should be 0
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
......@@ -629,7 +644,7 @@ void CameraSectionTest::_testScanForGimbalSection(void)
visualItems.clear();
invalidSimpleItem.missionItem() = _validGimbalItem->missionItem();
invalidSimpleItem.missionItem().setParam4(10); // alt is not supported
invalidSimpleItem.missionItem().setParam4(10); // alt is not supported, should be 0
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
......@@ -638,7 +653,7 @@ void CameraSectionTest::_testScanForGimbalSection(void)
visualItems.clear();
invalidSimpleItem.missionItem() = _validGimbalItem->missionItem();
invalidSimpleItem.missionItem().setParam5(10); // lat is not supported
invalidSimpleItem.missionItem().setParam5(10); // lat is not supported, should be 0
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
......@@ -647,7 +662,7 @@ void CameraSectionTest::_testScanForGimbalSection(void)
visualItems.clear();
invalidSimpleItem.missionItem() = _validGimbalItem->missionItem();
invalidSimpleItem.missionItem().setParam6(10); // lon is not supported
invalidSimpleItem.missionItem().setParam6(10); // lon is not supported, should be 0
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
......@@ -709,14 +724,27 @@ void CameraSectionTest::_testScanForCameraModeSection(void)
*/
// Mode command but incorrect settings
SimpleMissionItem invalidSimpleItem(_masterController, false /* flyView */, _validCameraPhotoModeItem->missionItem(), nullptr);
invalidSimpleItem.missionItem().setParam3(1); // Param3 should be NaN
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
QCOMPARE(_cameraSection->specifyCameraMode(), false);
QCOMPARE(_cameraSection->settingsSpecified(), false);
visualItems.clear();
std::function<void(MissionItem&, double)> rgSetParamFns[] = {
&MissionItem::setParam1,
&MissionItem::setParam2,
&MissionItem::setParam3,
&MissionItem::setParam4,
&MissionItem::setParam5,
&MissionItem::setParam6,
&MissionItem::setParam7
};
for (int fnIndex=2; fnIndex<7; fnIndex++) {
rgSetParamFns[fnIndex](invalidSimpleItem.missionItem(), 0); // should be NaN
visualItems.append(&invalidSimpleItem);
QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false);
QCOMPARE(visualItems.count(), 1);
QCOMPARE(_cameraSection->specifyCameraMode(), false);
QCOMPARE(_cameraSection->settingsSpecified(), false);
visualItems.clear();
}
}
void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void)
......@@ -1135,3 +1163,25 @@ SimpleMissionItem* CameraSectionTest::createValidStopTimeItem(PlanMasterControll
MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false),
parent);
}
SimpleMissionItem* CameraSectionTest::createInvalidStopVideoItem(PlanMasterController* masterController, QObject* parent)
{
SimpleMissionItem* invalidSimpleItem = createValidStopVideoItem(masterController, parent);
invalidSimpleItem->missionItem().setParam1(10); // must be 0 to be valid for scan
return invalidSimpleItem;
}
SimpleMissionItem* CameraSectionTest::createInvalidStopDistanceItem(PlanMasterController* masterController, QObject* parent)
{
SimpleMissionItem* invalidSimpleItem = createValidStopDistanceItem(masterController, parent);
invalidSimpleItem->missionItem().setParam2(-1); // Should be 0
return invalidSimpleItem;
}
SimpleMissionItem* CameraSectionTest::createInvalidStopTimeItem(PlanMasterController* masterController, QObject* parent)
{
SimpleMissionItem* invalidSimpleItem = createValidStopTimeItem(masterController, parent);
invalidSimpleItem->missionItem().setParam1(1); // Should be 0
return invalidSimpleItem;
}
src/MissionManager/CameraSectionTest.h
View file @ 48ffa39e
......@@ -24,9 +24,12 @@ public:
void init(void) override;
void cleanup(void) override;
static SimpleMissionItem* createValidStopVideoItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createValidStopDistanceItem(PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createValidStopTimeItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createValidStopVideoItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createValidStopDistanceItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createValidStopTimeItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createInvalidStopVideoItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createInvalidStopDistanceItem (PlanMasterController* masterController, QObject* parent);
static SimpleMissionItem* createInvalidStopTimeItem (PlanMasterController* masterController, QObject* parent);
private slots:
void _testDirty (void);
......
src/MissionManager/FWLandingPattern.FactMetaData.json
View file @ 48ffa39e
......@@ -5,75 +5,87 @@
[
{
"name": "LandingDistance",
"shortDesc": "Distance between landing and loiter points.",
"shortDesc": "Distance between approach and land points.",
"type": "double",
"units": "m",
"min": 10,
"decimalPlaces": 1,
"default": 300.0
"default": 300.0
},
{
"name": "LandingHeading",
"shortDesc": "Heading from loiter point to land point.",
"shortDesc": "Heading from approach to land point.",
"type": "double",
"units": "deg",
"min": 0.0,
"max": 360.0,
"decimalPlaces": 0,
"default": 270.0
"default": 270.0
},
{
"name": "LoiterAltitude",
"shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will proceed to land.",
"name": "FinalApproachAltitude",
"shortDesc": "Altitude to begin landing approach from.",
"type": "double",
"units": "m",
"decimalPlaces": 1,
"default": 40.0
"default": 40.0
},
{
"name": "LoiterRadius",
"shortDesc": "Loiter radius.",
"shortDesc": "Loiter radius.",
"type": "double",
"decimalPlaces": 1,
"min": 1,
"units": "m",
"default": 75.0
"default": 75.0
},
{
"name": "LoiterClockwise",
"shortDesc": "Loiter clockwise around the final approach point.",
"type": "bool",
"default": true
},
{
"name": "LandingAltitude",
"shortDesc": "Altitude for landing point.",
"shortDesc": "Altitude for landing point.",
"type": "double",
"units": "m",
"decimalPlaces": 1,
"default": 0.0
"default": 0.0
},
{
"name": "GlideSlope",
"shortDesc": "The glide slope between the loiter and landing point.",
"shortDesc": "The glide slope between the loiter and landing point.",
"type": "double",
"units": "deg",
"min": 0.1,
"max": 90,
"decimalPlaces": 1,
"default": 6.0
"default": 6.0
},
{
"name": "ValueSetIsDistance",
"shortDesc": "Value controller approach point is distance",
"type": "bool",
"default": false
},
{
"name": "ValueSetIsDistance",
"shortDesc": "Value controller loiter point is distance",
"type": "bool",
"default": false
"name": "UseLoiterToAlt",
"shortDesc": "Use a loiter to altitude item for final appoach. Otherwise use a regular waypoint.",
"type": "bool",
"default": true
},
{
"name": "StopTakingPhotos",
"shortDesc": "Stop taking photos",
"type": "bool",
"default": true
"name": "StopTakingPhotos",
"shortDesc": "Stop taking photos",
"type": "bool",
"default": true
},
{
"name": "StopTakingVideo",
"shortDesc": "Stop taking video",
"type": "bool",
"default": true
"name": "StopTakingVideo",
"shortDesc": "Stop taking video",
"type": "bool",
"default": true
}
]
}
src/MissionManager/FWLandingPatternTest.cc
View file @ 48ffa39e
......@@ -22,18 +22,15 @@ void FWLandingPatternTest::init(void)
{
VisualMissionItemTest::init();
rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
_fwItem = new FixedWingLandingComplexItem(_masterController, false /* flyView */, this);
_multiSpy = new MultiSignalSpy();
QCOMPARE(_multiSpy->init(_fwItem, rgSignals, cSignals), true);
_createSpy(_fwItem, &_viSpy);
// Start in a clean state
QVERIFY(!_fwItem->dirty());
_fwItem->setLandingCoordinate(QGeoCoordinate(47, -122));
_fwItem->setDirty(false);
QVERIFY(!_fwItem->dirty());
_multiSpy->clearAllSignals();
_viSpy->clearAllSignals();
_validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_masterController, this);
_validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_masterController, this);
......@@ -43,7 +40,7 @@ void FWLandingPatternTest::init(void)
void FWLandingPatternTest::cleanup(void)
{
delete _fwItem;
delete _multiSpy;
delete _viSpy;
delete _validStopVideoItem;
delete _validStopDistanceItem;
delete _validStopTimeItem;
......@@ -57,142 +54,35 @@ void FWLandingPatternTest::_testDefaults(void)
QCOMPARE(_fwItem->stopTakingVideo()->rawValue().toBool(), true);
}
void FWLandingPatternTest::_testItemCount(void)
{
QList<MissionItem*> items;
_fwItem->stopTakingPhotos()->setRawValue(true);
_fwItem->stopTakingVideo()->setRawValue(true);
_fwItem->appendMissionItems(items, this);
QCOMPARE(items.count(), 3 + CameraSection::stopTakingPhotosCommandCount() + CameraSection::stopTakingVideoCommandCount());
QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber());
items.clear();
_fwItem->stopTakingPhotos()->setRawValue(false);
_fwItem->stopTakingVideo()->setRawValue(false);
_fwItem->appendMissionItems(items, this);
QCOMPARE(items.count(), 3);
QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber());
items.clear();
}
void FWLandingPatternTest::_testAppendSectionItems(void)
{
QList<MissionItem*> rgMissionItems;
// Create the set of appended mission items
_fwItem->stopTakingPhotos()->setRawValue(true);
_fwItem->stopTakingVideo()->setRawValue(true);
_fwItem->appendMissionItems(rgMissionItems, this);
// Convert to visual items
QmlObjectListModel* simpleItems = new QmlObjectListModel(this);
for (MissionItem* item: rgMissionItems) {
SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems);
simpleItem->missionItem() = *item;
simpleItems->append(simpleItem);
}
// Scan the items back in to verify the same values come back
// Note that the compares does the best it can with doubles going to floats and back causing inaccuracies beyond a fuzzy compare.
QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _masterController));
QCOMPARE(simpleItems->count(), 1);
_validateItem(simpleItems->value<FixedWingLandingComplexItem*>(0));
// Reset
simpleItems->deleteLater();
rgMissionItems.clear();
// Check for no stop camera actions
_fwItem->stopTakingPhotos()->setRawValue(false);
_fwItem->stopTakingVideo()->setRawValue(false);
_fwItem->appendMissionItems(rgMissionItems, this);
simpleItems = new QmlObjectListModel(this);
for (MissionItem* item: rgMissionItems) {
SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems);
simpleItem->missionItem() = *item;
simpleItems->append(simpleItem);
}
QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _masterController));
QCOMPARE(simpleItems->count(), 1);
_validateItem(simpleItems->value<FixedWingLandingComplexItem*>(0));
// Reset
simpleItems->deleteLater();
rgMissionItems.clear();
}
void FWLandingPatternTest::_testDirty(void)
{
_fwItem->setDirty(true);
QVERIFY(_fwItem->dirty());
QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask));
QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
QVERIFY(_viSpy->checkOnlySignalByMask(dirtyChangedMask));
QVERIFY(_viSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_fwItem->setDirty(false);
_multiSpy->clearAllSignals();
_viSpy->clearAllSignals();
// These facts should set dirty when changed
QList<Fact*> rgFacts;
rgFacts << _fwItem->loiterAltitude()
<< _fwItem->landingHeading()
<< _fwItem->loiterRadius()
<< _fwItem->landingAltitude()
<< _fwItem->landingDistance()
<< _fwItem->glideSlope()
<< _fwItem->stopTakingPhotos()
<< _fwItem->stopTakingVideo()
rgFacts << _fwItem->glideSlope()
<< _fwItem->valueSetIsDistance();
for(Fact* fact: rgFacts) {
qDebug() << fact->name();
QVERIFY(!_fwItem->dirty());
if (fact->typeIsBool()) {
fact->setRawValue(!fact->rawValue().toBool());
} else {
fact->setRawValue(fact->rawValue().toDouble() + 1);
}
QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_fwItem->setDirty(false);
_multiSpy->clearAllSignals();
}
rgFacts.clear();
// These bool properties should set dirty when changed
QList<const char*> rgBoolNames;
rgBoolNames << "loiterClockwise"
<< "altitudesAreRelative";
const QMetaObject* metaObject = _fwItem->metaObject();
for(const char* boolName: rgBoolNames) {
qDebug() << boolName;
QVERIFY(!_fwItem->dirty());
QMetaProperty boolProp = metaObject->property(metaObject->indexOfProperty(boolName));
QVERIFY(boolProp.write(_fwItem, !boolProp.read(_fwItem).toBool()));
QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
changeFactValue(fact);
QVERIFY(_viSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_viSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_fwItem->setDirty(false);
_multiSpy->clearAllSignals();
_viSpy->clearAllSignals();
}
rgFacts.clear();
// These coordinates should set dirty when changed
QVERIFY(!_fwItem->dirty());
_fwItem->setLoiterCoordinate(_fwItem->loiterCoordinate().atDistanceAndAzimuth(1, 0));
QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_fwItem->setDirty(false);
_multiSpy->clearAllSignals();
QVERIFY(!_fwItem->dirty());
_fwItem->setLoiterCoordinate(_fwItem->landingCoordinate().atDistanceAndAzimuth(1, 0));
QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_fwItem->setDirty(false);
_multiSpy->clearAllSignals();
}
void FWLandingPatternTest::_testSaveLoad(void)
{
QJsonArray items;
QJsonArray items;
_fwItem->save(items);
QString errorString;
......@@ -211,20 +101,6 @@ void FWLandingPatternTest::_validateItem(FixedWingLandingComplexItem* newItem)
{
QVERIFY(newItem);
QVERIFY(fuzzyCompareLatLon(newItem->loiterCoordinate(), _fwItem->loiterCoordinate()));
QVERIFY(fuzzyCompareLatLon(newItem->loiterTangentCoordinate(), _fwItem->loiterTangentCoordinate()));
QVERIFY(fuzzyCompareLatLon(newItem->landingCoordinate(), _fwItem->landingCoordinate()));
QCOMPARE(newItem->stopTakingPhotos()->rawValue().toBool(), _fwItem->stopTakingPhotos()->rawValue().toBool());
QCOMPARE(newItem->stopTakingVideo()->rawValue().toBool(), _fwItem->stopTakingVideo()->rawValue().toBool());
QCOMPARE(newItem->loiterAltitude()->rawValue().toInt(), _fwItem->loiterAltitude()->rawValue().toInt());
QCOMPARE(newItem->loiterRadius()->rawValue().toInt(), _fwItem->loiterRadius()->rawValue().toInt());
QCOMPARE(newItem->landingAltitude()->rawValue().toInt(), _fwItem->landingAltitude()->rawValue().toInt());
QCOMPARE(newItem->landingHeading()->rawValue().toInt(), _fwItem->landingHeading()->rawValue().toInt());
QCOMPARE(newItem->landingDistance()->rawValue().toInt(), _fwItem->landingDistance()->rawValue().toInt());
QCOMPARE(newItem->glideSlope()->rawValue().toInt(), _fwItem->glideSlope()->rawValue().toInt());
QCOMPARE(newItem->valueSetIsDistance()->rawValue().toBool(), _fwItem->valueSetIsDistance()->rawValue().toBool());
QCOMPARE(newItem->_loiterClockwise, _fwItem->_loiterClockwise);
QCOMPARE(newItem->_altitudesAreRelative, _fwItem->_altitudesAreRelative);
QCOMPARE(newItem->_landingCoordSet, _fwItem->_landingCoordSet);
}
src/MissionManager/FWLandingPatternTest.h
View file @ 48ffa39e
......@@ -25,30 +25,16 @@ public:
void cleanup(void) override;
private slots:
void _testDirty (void);
void _testItemCount (void);
void _testDefaults (void);
void _testAppendSectionItems (void);
void _testSaveLoad (void);
void _testDirty (void);
void _testDefaults (void);
void _testSaveLoad (void);
private:
void _validateItem(FixedWingLandingComplexItem* newItem);
enum {
dirtyChangedIndex = 0,
maxSignalIndex,
};
enum {
dirtyChangedMask = 1 << dirtyChangedIndex,
};
static const size_t cSignals = maxSignalIndex;
const char* rgSignals[cSignals];
FixedWingLandingComplexItem* _fwItem = nullptr;
MultiSignalSpy* _multiSpy = nullptr;
SimpleMissionItem* _validStopVideoItem = nullptr;
SimpleMissionItem* _validStopDistanceItem = nullptr;
SimpleMissionItem* _validStopTimeItem = nullptr;
FixedWingLandingComplexItem* _fwItem = nullptr;
MultiSignalSpy* _viSpy = nullptr;
SimpleMissionItem* _validStopVideoItem = nullptr;
SimpleMissionItem* _validStopDistanceItem = nullptr;
SimpleMissionItem* _validStopTimeItem = nullptr;
};
src/MissionManager/FixedWingLandingComplexItem.cc
View file @ 48ffa39e
......@@ -21,101 +21,36 @@ QGC_LOGGING_CATEGORY(FixedWingLandingComplexItemLog, "FixedWingLandingComplexIte
const QString FixedWingLandingComplexItem::name(tr("Fixed Wing Landing"));
const char* FixedWingLandingComplexItem::settingsGroup = "FixedWingLanding";
const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern";
const char* FixedWingLandingComplexItem::settingsGroup = "FixedWingLanding";
const char* FixedWingLandingComplexItem::jsonComplexItemTypeValue = "fwLandingPattern";
const char* FixedWingLandingComplexItem::loiterToLandDistanceName = "LandingDistance";
const char* FixedWingLandingComplexItem::landingHeadingName = "LandingHeading";
const char* FixedWingLandingComplexItem::loiterAltitudeName = "LoiterAltitude";
const char* FixedWingLandingComplexItem::loiterRadiusName = "LoiterRadius";
const char* FixedWingLandingComplexItem::landingAltitudeName = "LandingAltitude";
const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope";
const char* FixedWingLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos";
const char* FixedWingLandingComplexItem::stopTakingVideoName = "StopTakingVideo";
const char* FixedWingLandingComplexItem::valueSetIsDistanceName = "ValueSetIsDistance";
const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope";
const char* FixedWingLandingComplexItem::valueSetIsDistanceName = "ValueSetIsDistance";
const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate";
const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius";
const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise";
const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate";
const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance";
const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative";
const char* FixedWingLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos";
const char* FixedWingLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos";
// Usage deprecated
const char* FixedWingLandingComplexItem::_jsonFallRateKey = "fallRate";
const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative";
const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative";
const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance";
FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
: LandingComplexItem (masterController, flyView, parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), this))
, _landingDistanceFact (settingsGroup, _metaDataMap[loiterToLandDistanceName])
, _loiterAltitudeFact (settingsGroup, _metaDataMap[loiterAltitudeName])
, _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
, _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
, _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName])
, _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName])
, _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName])
, _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName])
, _glideSlopeFact (settingsGroup, _metaDataMap[glideSlopeName])
, _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName])
, _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName])
, _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName])
, _valueSetIsDistanceFact (settingsGroup, _metaDataMap[valueSetIsDistanceName])
{
_editorQml = "qrc:/qml/FWLandingPatternEditor.qml";
_isIncomplete = false;
_editorQml = "qrc:/qml/FWLandingPatternEditor.qml";
_isIncomplete = false;
_init();
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged);
connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged);
connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::valueSetIsDistanceChanged, this, &FixedWingLandingComplexItem::_setDirty);
connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged);
connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_amslEntryAltChanged);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_amslExitAltChanged);
connect(this, &FixedWingLandingComplexItem::amslEntryAltChanged, this, &FixedWingLandingComplexItem::maxAMSLAltitudeChanged);
connect(this, &FixedWingLandingComplexItem::amslExitAltChanged, this, &FixedWingLandingComplexItem::minAMSLAltitudeChanged);
connect(this, &FixedWingLandingComplexItem::landingCoordSetChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged);
connect(this, &FixedWingLandingComplexItem::wizardModeChanged, this, &FixedWingLandingComplexItem::readyForSaveStateChanged);
connect(this, &FixedWingLandingComplexItem::loiterTangentCoordinateChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal);
// The follow is used to compress multiple recalc calls in a row to into a single call.
connect(this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal, this, &FixedWingLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly, Qt::QueuedConnection);
qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&FixedWingLandingComplexItem::_updateFlightPathSegmentsSignal));
if (_masterController->controllerVehicle()->apmFirmware()) {
// ArduPilot does not support camera commands
_stopTakingVideoFact.setRawValue(false);
_stopTakingPhotosFact.setRawValue(false);
}
connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged);
connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty);
if (_valueSetIsDistanceFact.rawValue().toBool()) {
_recalcFromHeadingAndDistanceChange();
......@@ -125,115 +60,32 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* m
setDirty(false);
}
int FixedWingLandingComplexItem::lastSequenceNumber(void) const
{
// Fixed items are:
// land start, loiter, land
// Optional items are:
// stop photos/video
return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0);
}
void FixedWingLandingComplexItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(_dirty);
}
}
void FixedWingLandingComplexItem::save(QJsonArray& missionItems)
{
QJsonObject saveObject;
saveObject[JsonHelper::jsonVersionKey] = 2;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
QGeoCoordinate coordinate;
QJsonValue jsonCoordinate;
coordinate = _loiterCoordinate;
coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate;
coordinate = _landingCoordinate;
coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonLandingCoordinateKey] = jsonCoordinate;
QJsonObject saveObject = _save();
saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble();
saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool();
saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool();
saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise;
saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative;
saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistanceFact.rawValue().toBool();
saveObject[JsonHelper::jsonVersionKey] = 2;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistanceFact.rawValue().toBool();
missionItems.append(saveObject);
}
void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber)
{
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(sequenceNumber);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
{ _jsonLoiterCoordinateKey, QJsonValue::Array, true },
{ _jsonLoiterRadiusKey, QJsonValue::Double, true },
{ _jsonLoiterClockwiseKey, QJsonValue::Bool, true },
{ _jsonLandingCoordinateKey, QJsonValue::Array, true },
{ _jsonStopTakingPhotosKey, QJsonValue::Bool, false },
{ _jsonStopTakingVideoKey, QJsonValue::Bool, false },
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}
QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
return false;
}
setSequenceNumber(sequenceNumber);
_ignoreRecalcSignals = true;
int version = complexObject[JsonHelper::jsonVersionKey].toInt();
if (version == 1) {
QList<JsonHelper::KeyValidateInfo> v1KeyInfoList = {
{ _jsonLoiterAltitudeRelativeKey, QJsonValue::Bool, true },
{ _jsonLandingAltitudeRelativeKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) {
return false;
}
bool loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool();
bool landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
if (loiterAltitudeRelative != landingAltitudeRelative) {
qgcApp()->showAppMessage(tr("Fixed Wing Landing Pattern: "
"Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. "
"Both have been set to altitude relative. Be sure to adjust/check your plan prior to flight."));
_altitudesAreRelative = true;
} else {
_altitudesAreRelative = loiterAltitudeRelative;
}
_valueSetIsDistanceFact.setRawValue(true);
} else if (version == 2) {
QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
{ _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true },
{ _jsonValueSetIsDistanceKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
......@@ -241,7 +93,6 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq
return false;
}
_altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool();
_valueSetIsDistanceFact.setRawValue(complexObject[_jsonValueSetIsDistanceKey].toBool());
} else {
errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
......@@ -249,82 +100,10 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq
return false;
}
QGeoCoordinate coordinate;
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_loiterCoordinate = coordinate;
_loiterAltitudeFact.setRawValue(coordinate.altitude());
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_landingCoordinate = coordinate;
_landingAltitudeFact.setRawValue(coordinate.altitude());
_loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
_loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool();
if (complexObject.contains(_jsonStopTakingPhotosKey)) {
_stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool());
} else {
_stopTakingPhotosFact.setRawValue(false);
}
if (complexObject.contains(_jsonStopTakingVideoKey)) {
_stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool());
} else {
_stopTakingVideoFact.setRawValue(false);
}
_calcGlideSlope();
_landingCoordSet = true;
_ignoreRecalcSignals = false;
_recalcFromCoordinateChange();
emit coordinateChanged(this->coordinate()); // This will kick off terrain query
return true;
}
double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
}
bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
{
return true;
}
MissionItem* FixedWingLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent)
{
return new MissionItem(seqNum, // sequence number
MAV_CMD_DO_LAND_START, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7
true, // autoContinue
false, // isCurrentItem
parent);
return _load(complexObject, sequenceNumber, jsonComplexItemTypeValue, version == 1 /* useDeprecatedRelAltKeys */, errorString);
}
MissionItem* FixedWingLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent)
{
return new MissionItem(seqNum,
MAV_CMD_NAV_LOITER_TO_ALT,
altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
1.0, // Heading required = true
loiterRadius, // Loiter radius
0.0, // param 3 - unused
1.0, // Exit crosstrack - tangent of loiter to land point
lat, lon, alt,
true, // autoContinue
false, // isCurrentItem
parent);
}
MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
MissionItem* FixedWingLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
{
return new MissionItem(seqNum,
MAV_CMD_NAV_LAND,
......@@ -337,202 +116,10 @@ MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel
}
void FixedWingLandingComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
int seqNum = _sequenceNumber;
// IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem
MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent);
items.append(item);
if (_stopTakingPhotosFact.rawValue().toBool()) {
CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent);
}
if (_stopTakingVideoFact.rawValue().toBool()) {
CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent);
}
double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0);
item = createLoiterToAltItem(seqNum++,
_altitudesAreRelative,
loiterRadius,
_loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(),
missionItemParent);
items.append(item);
item = createLandItem(seqNum++,
_altitudesAreRelative,
_landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(),
missionItemParent);
items.append(item);
}
bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
{
qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count();
if (visualItems->count() < 3) {
return false;
}
// A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list:
// MAV_CMD_DO_LAND_START - required
// Stop taking photos sequence - optional
// Stop taking video sequence - optional
// MAV_CMD_NAV_LOITER_TO_ALT - required
// MAV_CMD_NAV_LAND - required
// Start looking for the commands in reverse order from the end of the list
int scanIndex = visualItems->count() - 1;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex--);
if (!item) {
return false;
}
MissionItem& missionItemLand = item->missionItem();
if (missionItemLand.command() != MAV_CMD_NAV_LAND ||
!(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) ||
missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) {
return false;
}
MAV_FRAME landPointFrame = missionItemLand.frame();
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
MissionItem& missionItemLoiter = item->missionItem();
if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT ||
missionItemLoiter.frame() != landPointFrame ||
missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) {
return false;
}
scanIndex -= CameraSection::stopTakingVideoCommandCount();
bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingVideo) {
scanIndex += CameraSection::stopTakingVideoCommandCount();
}
scanIndex -= CameraSection::stopTakingPhotosCommandCount();
bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingPhotos) {
scanIndex += CameraSection::stopTakingPhotosCommandCount();
}
scanIndex--;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
MissionItem& missionItemDoLandStart = item->missionItem();
if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
missionItemDoLandStart.frame() != MAV_FRAME_MISSION ||
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) {
return false;
}
// We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission.
// Since we have scanned it we need to remove the items for it fromt the list
int deleteCount = 3;
if (stopTakingPhotos) {
deleteCount += CameraSection::stopTakingPhotosCommandCount();
}
if (stopTakingVideo) {
deleteCount += CameraSection::stopTakingVideoCommandCount();
}
int firstItem = visualItems->count() - deleteCount;
while (deleteCount--) {
visualItems->removeAt(firstItem)->deleteLater();
}
// Now stuff all the scanned information into the item
FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(masterController, flyView, visualItems);
complexItem->_ignoreRecalcSignals = true;
complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2()));
complexItem->_loiterClockwise = missionItemLoiter.param2() > 0;
complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6()));
complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7());
complexItem->_landingCoordinate.setLatitude(missionItemLand.param5());
complexItem->_landingCoordinate.setLongitude(missionItemLand.param6());
complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7());
complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos);
complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo);
complexItem->_calcGlideSlope();
complexItem->_landingCoordSet = true;
complexItem->_ignoreRecalcSignals = false;
complexItem->_recalcFromCoordinateChange();
complexItem->setDirty(false);
visualItems->append(complexItem);
return true;
}
double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle)
{
double heading = (angle - 90) * -1;
if (heading < 0) {
heading += 360;
}
return heading;
}
double FixedWingLandingComplexItem::_headingToMathematicAngle(double heading)
{
return heading - 90 * -1;
}
void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
}
void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
_landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
emit landingCoordinateChanged(_landingCoordinate);
}
void FixedWingLandingComplexItem::_setDirty(void)
{
setDirty(true);
}
void FixedWingLandingComplexItem::applyNewAltitude(double newAltitude)
{
_loiterAltitudeFact.setRawValue(newAltitude);
}
void FixedWingLandingComplexItem::_glideSlopeChanged(void)
{
if (!_ignoreRecalcSignals) {
double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
double glideSlope = _glideSlopeFact.rawValue().toDouble();
_landingDistanceFact.setRawValue(landingAltDifference / qTan(qDegreesToRadians(glideSlope)));
}
......@@ -540,22 +127,12 @@ void FixedWingLandingComplexItem::_glideSlopeChanged(void)
void FixedWingLandingComplexItem::_calcGlideSlope(void)
{
double landingAltDifference = _loiterAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
double landingAltDifference = _finalApproachAltitudeFact.rawValue().toDouble() - _landingAltitudeFact.rawValue().toDouble();
double landingDistance = _landingDistanceFact.rawValue().toDouble();
_glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance)));
}
void FixedWingLandingComplexItem::_signalLastSequenceNumberChanged(void)
{
emit lastSequenceNumberChanged(lastSequenceNumber());
}
FixedWingLandingComplexItem::ReadyForSaveState FixedWingLandingComplexItem::readyForSaveState(void) const
{
return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData;
}
void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coordinate)
{
double savedHeading = landingHeading()->rawValue().toDouble();
......@@ -566,13 +143,18 @@ void FixedWingLandingComplexItem::moveLandingPosition(const QGeoCoordinate& coor
landingDistance()->setRawValue(savedDistance);
}
double FixedWingLandingComplexItem::amslEntryAlt(void) const
bool FixedWingLandingComplexItem::_isValidLandItem(const MissionItem& missionItem)
{
return _loiterAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
if (missionItem.command() != MAV_CMD_NAV_LAND ||
!(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) ||
missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0) {
return false;
} else {
return true;
}
}
double FixedWingLandingComplexItem::amslExitAlt(void) const
bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
{
return _landingAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem);
}
src/MissionManager/FixedWingLandingComplexItem.h
View file @ 48ffa39e
......@@ -32,114 +32,63 @@ public:
Q_INVOKABLE void moveLandingPosition(const QGeoCoordinate& coordinate); // Maintains the current landing distance and heading
Fact* loiterAltitude (void) final { return &_loiterAltitudeFact; }
Fact* loiterRadius (void) final { return &_loiterRadiusFact; }
Fact* landingAltitude (void) final { return &_landingAltitudeFact; }
Fact* landingDistance (void) final { return &_landingDistanceFact; }
Fact* landingHeading (void) final { return &_landingHeadingFact; }
Fact* stopTakingPhotos (void) final { return &_stopTakingPhotosFact; }
Fact* stopTakingVideo (void) final { return &_stopTakingVideoFact; }
Fact* glideSlope (void) { return &_glideSlopeFact; }
Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; }
/// Scans the loaded items for a landing pattern complex item
static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController);
static MissionItem* createDoLandStartItem (int seqNum, QObject* parent);
static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent);
static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent);
// Overrides from ComplexMissionItem
QString patternName (void) const final { return name; }
int lastSequenceNumber (void) const final;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
double greatestDistanceTo (const QGeoCoordinate &other) const final;
QString mapVisualQML (void) const final { return QStringLiteral("FWLandingPatternMapVisual.qml"); }
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final;
bool specifiesAltitudeOnly (void) const final { return false; }
QString commandDescription (void) const final { return "Landing Pattern"; }
QString commandName (void) const final { return "Landing Pattern"; }
QString abbreviation (void) const final { return "L"; }
QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double additionalTimeDelay (void) const final { return 0; }
ReadyForSaveState readyForSaveState (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonArray& missionItems) final;
double amslEntryAlt (void) const final;
double amslExitAlt (void) const final;
double minAMSLAltitude (void) const final { return amslExitAlt(); }
double maxAMSLAltitude (void) const final { return amslEntryAlt(); }
static const QString name;
static const char* jsonComplexItemTypeValue;
static const char* settingsGroup;
static const char* loiterToLandDistanceName;
static const char* loiterAltitudeName;
static const char* loiterRadiusName;
static const char* landingHeadingName;
static const char* landingAltitudeName;
static const char* glideSlopeName;
static const char* stopTakingPhotosName;
static const char* stopTakingVideoName;
static const char* valueSetIsDistanceName;
signals:
void valueSetIsDistanceChanged (bool valueSetIsDistance);
private slots:
void _updateLoiterCoodinateAltitudeFromFact (void);
void _updateLandingCoodinateAltitudeFromFact (void);
double _mathematicAngleToHeading (double angle);
double _headingToMathematicAngle (double heading);
void _setDirty (void);
void _glideSlopeChanged (void);
void _signalLastSequenceNumberChanged (void);
void _glideSlopeChanged(void);
private:
void _calcGlideSlope(void) final;
static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new FixedWingLandingComplexItem(masterController, flyView, parent); }
static bool _isValidLandItem(const MissionItem& missionItem);
// Overrides from LandingComplexItem
const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; }
const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; }
const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; }
const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; }
const Fact* _landingDistance (void) const final { return &_landingDistanceFact; }
const Fact* _landingHeading (void) const final { return &_landingHeadingFact; }
const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; }
const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; }
const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; }
void _calcGlideSlope (void) final;
MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final;
QMap<QString, FactMetaData*> _metaDataMap;
Fact _landingDistanceFact;
Fact _loiterAltitudeFact;
Fact _finalApproachAltitudeFact;
Fact _loiterRadiusFact;
Fact _loiterClockwiseFact;
Fact _landingHeadingFact;
Fact _landingAltitudeFact;
Fact _glideSlopeFact;
Fact _useLoiterToAltFact;
Fact _stopTakingPhotosFact;
Fact _stopTakingVideoFact;
Fact _valueSetIsDistanceFact;
static const char* _jsonLoiterCoordinateKey;
static const char* _jsonLoiterRadiusKey;
static const char* _jsonLoiterClockwiseKey;
static const char* _jsonLandingCoordinateKey;
static const char* _jsonValueSetIsDistanceKey;
static const char* _jsonAltitudesAreRelativeKey;
static const char* _jsonStopTakingPhotosKey;
static const char* _jsonStopTakingVideoKey;
// Only in older file formats
static const char* _jsonLandingAltitudeRelativeKey;
static const char* _jsonLoiterAltitudeRelativeKey;
static const char* _jsonFallRateKey;
friend FWLandingPatternTest;
};
......
src/MissionManager/LandingComplexItem.cc
View file @ 48ffa39e
......@@ -20,6 +20,36 @@
QGC_LOGGING_CATEGORY(LandingComplexItemLog, "LandingComplexItemLog")
const char* LandingComplexItem::_jsonFinalApproachCoordinateKey = "landingApproachCoordinate";
const char* LandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius";
const char* LandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise";
const char* LandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate";
const char* LandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative";
const char* LandingComplexItem::_jsonUseLoiterToAltKey = "stopTakingPhotos";
const char* LandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos";
const char* LandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos";
const char* LandingComplexItem::finalApproachToLandDistanceName = "LandingDistance";
const char* LandingComplexItem::landingHeadingName = "LandingHeading";
const char* LandingComplexItem::finalApproachAltitudeName = "FinalApproachAltitude";
const char* LandingComplexItem::loiterRadiusName = "LoiterRadius";
const char* LandingComplexItem::loiterClockwiseName = "LoiterClockwise";
const char* LandingComplexItem::landingAltitudeName = "LandingAltitude";
const char* LandingComplexItem::useLoiterToAltName = "UseLoiterToAlt";
const char* LandingComplexItem::stopTakingPhotosName = "StopTakingPhotos";
const char* LandingComplexItem::stopTakingVideoName = "StopTakingVideo";
// Deprecated keys
// Support for separate relative alt settings for land/loiter was removed. It now only has a single
// relative alt setting stored in _jsonAltitudesAreRelativeKey.
const char* LandingComplexItem::_jsonDeprecatedLandingAltitudeRelativeKey = "landAltitudeRelative";
const char* LandingComplexItem::_jsonDeprecatedLoiterAltitudeRelativeKey = "loiterAltitudeRelative";
// Name changed from _jsonDeprecatedLoiterCoordinateKey to _jsonFinalApproachCoordinateKey to reflect
// the new support for using either a loiter or just a waypoint as the approach entry point.
const char* LandingComplexItem::_jsonDeprecatedLoiterCoordinateKey = "loiterCoordinate";
LandingComplexItem::LandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
: ComplexMissionItem (masterController, flyView, parent)
{
......@@ -32,14 +62,66 @@ LandingComplexItem::LandingComplexItem(PlanMasterController* masterController, b
void LandingComplexItem::_init(void)
{
connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
if (_masterController->controllerVehicle()->apmFirmware()) {
// ArduPilot does not support camera commands
stopTakingVideo()->setRawValue(false);
stopTakingPhotos()->setRawValue(false);
}
connect(this, &LandingComplexItem::loiterCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(this, &LandingComplexItem::useLoiterToAltChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(loiterClockwise(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(useLoiterToAlt(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(stopTakingPhotos(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(stopTakingVideo(), &Fact::valueChanged, this, &LandingComplexItem::_setDirty);
connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_setDirty);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_setDirty);
connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_setDirty);
connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_setDirty);
connect(this, &LandingComplexItem::useLoiterToAltChanged, this, &LandingComplexItem::_setDirty);
connect(stopTakingPhotos(), &Fact::valueChanged, this, &LandingComplexItem::_signalLastSequenceNumberChanged);
connect(stopTakingVideo(), &Fact::valueChanged, this, &LandingComplexItem::_signalLastSequenceNumberChanged);
connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_amslEntryAltChanged);
connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_amslExitAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_amslEntryAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_amslExitAltChanged);
connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_amslEntryAltChanged);
connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_amslExitAltChanged);
connect(this, &LandingComplexItem::amslEntryAltChanged, this, &LandingComplexItem::maxAMSLAltitudeChanged);
connect(this, &LandingComplexItem::amslExitAltChanged, this, &LandingComplexItem::minAMSLAltitudeChanged);
connect(this, &LandingComplexItem::landingCoordSetChanged, this, &LandingComplexItem::readyForSaveStateChanged);
connect(this, &LandingComplexItem::wizardModeChanged, this, &LandingComplexItem::readyForSaveStateChanged);
connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::complexDistanceChanged);
connect(this, &LandingComplexItem::loiterTangentCoordinateChanged,this, &LandingComplexItem::complexDistanceChanged);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::complexDistanceChanged);
connect(this, &LandingComplexItem::loiterTangentCoordinateChanged,this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &LandingComplexItem::finalApproachCoordinateChanged,this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &LandingComplexItem::altitudesAreRelativeChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &LandingComplexItem::_updateFlightPathSegmentsSignal);
connect(finalApproachAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateFinalApproachCoodinateAltitudeFromFact);
connect(landingAltitude(), &Fact::valueChanged, this, &LandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
}
void LandingComplexItem::setLandingHeadingToTakeoffHeading()
......@@ -53,7 +135,7 @@ void LandingComplexItem::setLandingHeadingToTakeoffHeading()
double LandingComplexItem::complexDistance(void) const
{
return loiterCoordinate().distanceTo(loiterTangentCoordinate()) + loiterTangentCoordinate().distanceTo(landingCoordinate());
return finalApproachCoordinate().distanceTo(loiterTangentCoordinate()) + loiterTangentCoordinate().distanceTo(landingCoordinate());
}
// Never call this method directly. If you want to update the flight segments you emit _updateFlightPathSegmentsSignal()
......@@ -66,7 +148,7 @@ void LandingComplexItem::_updateFlightPathSegmentsDontCallDirectly(void)
_flightPathSegments.beginReset();
_flightPathSegments.clearAndDeleteContents();
_appendFlightPathSegment(loiterCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt());
_appendFlightPathSegment(finalApproachCoordinate(), amslEntryAlt(), loiterTangentCoordinate(), amslEntryAlt());
_appendFlightPathSegment(loiterTangentCoordinate(), amslEntryAlt(), landingCoordinate(), amslEntryAlt());
_appendFlightPathSegment(landingCoordinate(), amslEntryAlt(), landingCoordinate(), amslExitAlt());
_flightPathSegments.endReset();
......@@ -97,12 +179,12 @@ void LandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate)
}
}
void LandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate)
void LandingComplexItem::setFinalApproachCoordinate(const QGeoCoordinate& coordinate)
{
if (coordinate != _loiterCoordinate) {
_loiterCoordinate = coordinate;
if (coordinate != _finalApproachCoordinate) {
_finalApproachCoordinate = coordinate;
emit coordinateChanged(coordinate);
emit loiterCoordinateChanged(coordinate);
emit finalApproachCoordinateChanged(coordinate);
}
}
......@@ -139,13 +221,13 @@ void LandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);
// Loiter coord is 90 degrees counter clockwise from tangent coord
_loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90);
_loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble());
_finalApproachCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90);
_finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
emit finalApproachCoordinateChanged(_finalApproachCoordinate);
emit coordinateChanged(_finalApproachCoordinate);
_calcGlideSlope();
_ignoreRecalcSignals = false;
}
......@@ -168,10 +250,10 @@ void LandingComplexItem::_recalcFromRadiusChange(void)
double landToTangentDistance = landingDistance()->rawValue().toDouble();
double heading = landingHeading()->rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterDistance = _landingCoordinate.distanceTo(_finalApproachCoordinate);
if (landToLoiterDistance < radius) {
// Degnenerate case: Move tangent to loiter point
_loiterTangentCoordinate = _loiterCoordinate;
_loiterTangentCoordinate = _finalApproachCoordinate;
double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
......@@ -181,14 +263,14 @@ void LandingComplexItem::_recalcFromRadiusChange(void)
_ignoreRecalcSignals = false;
} else {
double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2));
double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1);
double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise()->rawValue().toBool() ? -1 : 1);
_loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent);
_loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble());
_finalApproachCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent);
_finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
emit finalApproachCoordinateChanged(_finalApproachCoordinate);
emit coordinateChanged(_finalApproachCoordinate);
_ignoreRecalcSignals = false;
}
}
......@@ -209,16 +291,16 @@ void LandingComplexItem::_recalcFromCoordinateChange(void)
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = loiterRadius()->rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
double landToLoiterDistance = _landingCoordinate.distanceTo(_finalApproachCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_finalApproachCoordinate);
double landToTangentDistance;
if (landToLoiterDistance < radius) {
// Degenerate case, set tangent to loiter coordinate
_loiterTangentCoordinate = _loiterCoordinate;
_loiterTangentCoordinate = _finalApproachCoordinate;
landToTangentDistance = _landingCoordinate.distanceTo(_loiterTangentCoordinate);
} else {
double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1);
double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise()->rawValue().toBool() ? 1 : -1);
landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2));
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle);
......@@ -236,4 +318,419 @@ void LandingComplexItem::_recalcFromCoordinateChange(void)
}
}
int LandingComplexItem::lastSequenceNumber(void) const
{
// Fixed items are:
// land start, loiter, land
// Optional items are:
// stop photos/video
return _sequenceNumber + 2 + (stopTakingPhotos()->rawValue().toBool() ? 2 : 0) + (stopTakingVideo()->rawValue().toBool() ? 1 : 0);
}
void LandingComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
{
int seqNum = _sequenceNumber;
// IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem
MissionItem* item = _createDoLandStartItem(seqNum++, missionItemParent);
items.append(item);
if (stopTakingPhotos()->rawValue().toBool()) {
CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent);
}
if (stopTakingVideo()->rawValue().toBool()) {
CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent);
}
item = _createFinalApproachItem(seqNum++, missionItemParent);
items.append(item);
item = _createLandItem(seqNum++,
_altitudesAreRelative,
_landingCoordinate.latitude(), _landingCoordinate.longitude(), landingAltitude()->rawValue().toDouble(),
missionItemParent);
items.append(item);
}
MissionItem* LandingComplexItem::_createDoLandStartItem(int seqNum, QObject* parent)
{
return new MissionItem(seqNum, // sequence number
MAV_CMD_DO_LAND_START, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7
true, // autoContinue
false, // isCurrentItem
parent);
}
MissionItem* LandingComplexItem::_createFinalApproachItem(int seqNum, QObject* parent)
{
if (useLoiterToAlt()->rawValue().toBool()) {
return new MissionItem(seqNum,
MAV_CMD_NAV_LOITER_TO_ALT,
_altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
1.0, // Heading required = true
loiterRadius()->rawValue().toDouble() * (_loiterClockwise()->rawValue().toBool() ? 1.0 : -1.0),
0.0, // param 3 - unused
1.0, // Exit crosstrack - tangent of loiter to land point
_finalApproachCoordinate.latitude(),
_finalApproachCoordinate.longitude(),
_finalApproachAltitude()->rawValue().toFloat(),
true, // autoContinue
false, // isCurrentItem
parent);
} else {
return new MissionItem(seqNum,
MAV_CMD_NAV_WAYPOINT,
_altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
0, // No hold time
0, // Use default acceptance radius
0, // Pass through waypoint
qQNaN(), // Yaw not specified
_finalApproachCoordinate.latitude(),
_finalApproachCoordinate.longitude(),
_finalApproachAltitude()->rawValue().toFloat(),
true, // autoContinue
false, // isCurrentItem
parent);
}
}
bool LandingComplexItem::_scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc)
{
qCDebug(LandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count();
if (visualItems->count() < 3) {
return false;
}
// A valid landing pattern is comprised of the follow commands in this order at the end of the item list:
// MAV_CMD_DO_LAND_START - required
// Stop taking photos sequence - optional
// Stop taking video sequence - optional
// MAV_CMD_NAV_LOITER_TO_ALT or MAV_CMD_NAV_WAYPOINT
// MAV_CMD_NAV_LAND or MAV_CMS_NAV_VTOL_LAND
// Start looking for the commands in reverse order from the end of the list
int scanIndex = visualItems->count() - 1;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex--);
if (!item) {
return false;
}
MissionItem& missionItemLand = item->missionItem();
if (!isLandItemFunc(missionItemLand)) {
return false;
}
MAV_FRAME landPointFrame = missionItemLand.frame();
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
bool useLoiterToAlt = true;
MissionItem& missionItemFinalApproach = item->missionItem();
if (missionItemFinalApproach.command() == MAV_CMD_NAV_LOITER_TO_ALT) {
if (missionItemFinalApproach.frame() != landPointFrame ||
missionItemFinalApproach.param1() != 1.0 || missionItemFinalApproach.param3() != 0 || missionItemFinalApproach.param4() != 1.0) {
return false;
}
} else if (missionItemFinalApproach.command() == MAV_CMD_NAV_WAYPOINT) {
if (missionItemFinalApproach.frame() != landPointFrame ||
missionItemFinalApproach.param1() != 0 || missionItemFinalApproach.param2() != 0 || missionItemFinalApproach.param3() != 0 ||
!qIsNaN(missionItemFinalApproach.param4()) ||
qIsNaN(missionItemFinalApproach.param5()) || qIsNaN(missionItemFinalApproach.param6()) || qIsNaN(missionItemFinalApproach.param6())) {
return false;
}
useLoiterToAlt = false;
} else {
return false;
}
scanIndex -= CameraSection::stopTakingVideoCommandCount();
bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingVideo) {
scanIndex += CameraSection::stopTakingVideoCommandCount();
}
scanIndex -= CameraSection::stopTakingPhotosCommandCount();
bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingPhotos) {
scanIndex += CameraSection::stopTakingPhotosCommandCount();
}
scanIndex--;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
MissionItem& missionItemDoLandStart = item->missionItem();
if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
missionItemDoLandStart.frame() != MAV_FRAME_MISSION ||
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0 ||
missionItemDoLandStart.param5() != 0 || missionItemDoLandStart.param6() != 0 || missionItemDoLandStart.param7() != 0) {
return false;
}
// We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission.
// Since we have scanned it we need to remove the items for it fromt the list
int deleteCount = 3;
if (stopTakingPhotos) {
deleteCount += CameraSection::stopTakingPhotosCommandCount();
}
if (stopTakingVideo) {
deleteCount += CameraSection::stopTakingVideoCommandCount();
}
int firstItem = visualItems->count() - deleteCount;
while (deleteCount--) {
visualItems->removeAt(firstItem)->deleteLater();
}
// Now stuff all the scanned information into the item
LandingComplexItem* complexItem = createItemFunc(masterController, flyView, visualItems /* parent */);
complexItem->_ignoreRecalcSignals = true;
complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
complexItem->setFinalApproachCoordinate(QGeoCoordinate(missionItemFinalApproach.param5(), missionItemFinalApproach.param6()));
complexItem->finalApproachAltitude()->setRawValue(missionItemFinalApproach.param7());
complexItem->useLoiterToAlt()->setRawValue(useLoiterToAlt);
if (useLoiterToAlt) {
complexItem->loiterRadius()->setRawValue(qAbs(missionItemFinalApproach.param2()));
complexItem->loiterClockwise()->setRawValue(missionItemFinalApproach.param2() > 0);
}
complexItem->_landingCoordinate.setLatitude(missionItemLand.param5());
complexItem->_landingCoordinate.setLongitude(missionItemLand.param6());
complexItem->landingAltitude()->setRawValue(missionItemLand.param7());
complexItem->stopTakingPhotos()->setRawValue(stopTakingPhotos);
complexItem->stopTakingVideo()->setRawValue(stopTakingVideo);
complexItem->_landingCoordSet = true;
complexItem->_ignoreRecalcSignals = false;
complexItem->_recalcFromCoordinateChange();
complexItem->setDirty(false);
visualItems->append(complexItem);
return true;
}
void LandingComplexItem::applyNewAltitude(double newAltitude)
{
finalApproachAltitude()->setRawValue(newAltitude);
}
LandingComplexItem::ReadyForSaveState LandingComplexItem::readyForSaveState(void) const
{
return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData;
}
void LandingComplexItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(_dirty);
}
}
void LandingComplexItem::_setDirty(void)
{
setDirty(true);
}
void LandingComplexItem::setSequenceNumber(int sequenceNumber)
{
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(sequenceNumber);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
double LandingComplexItem::amslEntryAlt(void) const
{
return finalApproachAltitude()->rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
}
double LandingComplexItem::amslExitAlt(void) const
{
return landingAltitude()->rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
}
void LandingComplexItem::_signalLastSequenceNumberChanged(void)
{
emit lastSequenceNumberChanged(lastSequenceNumber());
}
void LandingComplexItem::_updateFinalApproachCoodinateAltitudeFromFact(void)
{
_finalApproachCoordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble());
emit finalApproachCoordinateChanged(_finalApproachCoordinate);
emit coordinateChanged(_finalApproachCoordinate);
}
void LandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
_landingCoordinate.setAltitude(landingAltitude()->rawValue().toDouble());
emit landingCoordinateChanged(_landingCoordinate);
}
double LandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
return qMax(_finalApproachCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
}
QJsonObject LandingComplexItem::_save(void)
{
QJsonObject saveObject;
QGeoCoordinate coordinate;
QJsonValue jsonCoordinate;
coordinate = _finalApproachCoordinate;
coordinate.setAltitude(finalApproachAltitude()->rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonFinalApproachCoordinateKey] = jsonCoordinate;
coordinate = _landingCoordinate;
coordinate.setAltitude(landingAltitude()->rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonLandingCoordinateKey] = jsonCoordinate;
saveObject[_jsonLoiterRadiusKey] = loiterRadius()->rawValue().toDouble();
saveObject[_jsonStopTakingPhotosKey] = stopTakingPhotos()->rawValue().toBool();
saveObject[_jsonStopTakingVideoKey] = stopTakingVideo()->rawValue().toBool();
saveObject[_jsonLoiterClockwiseKey] = loiterClockwise()->rawValue().toBool();
saveObject[_jsonUseLoiterToAltKey] = useLoiterToAlt()->rawValue().toBool();
saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative;
return saveObject;
}
bool LandingComplexItem::_load(const QJsonObject& complexObject, int sequenceNumber, const QString& jsonComplexItemTypeValue, bool useDeprecatedRelAltKeys, QString& errorString)
{
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
{ _jsonDeprecatedLoiterCoordinateKey, QJsonValue::Array, false }, // Loiter changed to Final Approach
{ _jsonFinalApproachCoordinateKey, QJsonValue::Array, false },
{ _jsonLoiterRadiusKey, QJsonValue::Double, true },
{ _jsonLoiterClockwiseKey, QJsonValue::Bool, true },
{ _jsonLandingCoordinateKey, QJsonValue::Array, true },
{ _jsonStopTakingPhotosKey, QJsonValue::Bool, false },
{ _jsonStopTakingVideoKey, QJsonValue::Bool, false },
{ _jsonUseLoiterToAltKey, QJsonValue::Bool, false },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}
if (!complexObject.contains(_jsonDeprecatedLoiterCoordinateKey) && !complexObject.contains(_jsonFinalApproachCoordinateKey)) {
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ _jsonFinalApproachCoordinateKey, QJsonValue::Array, true },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}
}
QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
return false;
}
setSequenceNumber(sequenceNumber);
_ignoreRecalcSignals = true;
if (useDeprecatedRelAltKeys) {
QList<JsonHelper::KeyValidateInfo> v1KeyInfoList = {
{ _jsonDeprecatedLoiterAltitudeRelativeKey, QJsonValue::Bool, true },
{ _jsonDeprecatedLandingAltitudeRelativeKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, v1KeyInfoList, errorString)) {
return false;
}
bool loiterAltitudeRelative = complexObject[_jsonDeprecatedLoiterAltitudeRelativeKey].toBool();
bool landingAltitudeRelative = complexObject[_jsonDeprecatedLandingAltitudeRelativeKey].toBool();
if (loiterAltitudeRelative != landingAltitudeRelative) {
qgcApp()->showAppMessage(tr("Fixed Wing Landing Pattern: "
"Setting the loiter and landing altitudes with different settings for altitude relative is no longer supported. "
"Both have been set to relative altitude. Be sure to adjust/check your plan prior to flight."));
_altitudesAreRelative = true;
} else {
_altitudesAreRelative = loiterAltitudeRelative;
}
} else {
QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
{ _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
_ignoreRecalcSignals = false;
return false;
}
_altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool();
}
QGeoCoordinate coordinate;
QString finalApproachKey = complexObject.contains(_jsonFinalApproachCoordinateKey) ? _jsonFinalApproachCoordinateKey : _jsonDeprecatedLoiterCoordinateKey;
if (!JsonHelper::loadGeoCoordinate(complexObject[finalApproachKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_finalApproachCoordinate = coordinate;
finalApproachAltitude()->setRawValue(coordinate.altitude());
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_landingCoordinate = coordinate;
landingAltitude()->setRawValue(coordinate.altitude());
loiterRadius()->setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
loiterClockwise()->setRawValue(complexObject[_jsonLoiterClockwiseKey].toBool());
useLoiterToAlt()->setRawValue(complexObject[_jsonUseLoiterToAltKey].toBool(true));
stopTakingPhotos()->setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool(false));
stopTakingVideo()->setRawValue(complexObject[_jsonStopTakingVideoKey].toBool(false));
_calcGlideSlope();
_landingCoordSet = true;
_ignoreRecalcSignals = false;
_recalcFromCoordinateChange();
emit coordinateChanged(this->coordinate()); // This will kick off terrain query
return true;
}
void LandingComplexItem::setAltitudesAreRelative(bool altitudesAreRelative)
{
if (altitudesAreRelative != _altitudesAreRelative) {
_altitudesAreRelative = altitudesAreRelative;
emit altitudesAreRelativeChanged(_altitudesAreRelative);
}
}
src/MissionManager/LandingComplexItem.h
View file @ 48ffa39e
......@@ -17,6 +17,7 @@
Q_DECLARE_LOGGING_CATEGORY(LandingComplexItemLog)
class PlanMasterController;
class LandingComplexItemTest;
// Base class for landing patterns complex items.
class LandingComplexItem : public ComplexMissionItem
......@@ -26,47 +27,103 @@ class LandingComplexItem : public ComplexMissionItem
public:
LandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent);
Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT)
Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT)
Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT)
Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT)
Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT)
Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged)
Q_PROPERTY(bool altitudesAreRelative MEMBER _altitudesAreRelative NOTIFY altitudesAreRelativeChanged)
Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT)
Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT)
Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged)
Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged)
Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged)
Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged)
Q_PROPERTY(Fact* finalApproachAltitude READ finalApproachAltitude CONSTANT)
Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT)
Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT)
Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT)
Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT)
Q_PROPERTY(Fact* loiterClockwise READ loiterClockwise CONSTANT)
Q_PROPERTY(Fact* useLoiterToAlt READ useLoiterToAlt CONSTANT)
Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT)
Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT)
Q_PROPERTY(QGeoCoordinate finalApproachCoordinate READ finalApproachCoordinate WRITE setFinalApproachCoordinate NOTIFY finalApproachCoordinateChanged)
Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged)
Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged)
Q_PROPERTY(bool altitudesAreRelative READ altitudesAreRelative WRITE setAltitudesAreRelative NOTIFY altitudesAreRelativeChanged)
Q_PROPERTY(bool landingCoordSet READ landingCoordSet NOTIFY landingCoordSetChanged)
Q_INVOKABLE void setLandingHeadingToTakeoffHeading();
// These are virtual so the Facts can be created with their own specific FactGroup metadata
virtual Fact* loiterAltitude (void) = 0;
virtual Fact* loiterRadius (void) = 0;
virtual Fact* landingAltitude (void) = 0;
virtual Fact* landingDistance (void) = 0;
virtual Fact* landingHeading (void) = 0;
virtual Fact* stopTakingPhotos (void) = 0;
virtual Fact* stopTakingVideo (void) = 0;
const Fact* finalApproachAltitude (void) const { return _finalApproachAltitude(); }
const Fact* loiterRadius (void) const { return _loiterRadius(); }
const Fact* loiterClockwise (void) const { return _loiterClockwise(); }
const Fact* landingAltitude (void) const { return _landingAltitude(); }
const Fact* landingDistance (void) const { return _landingDistance(); }
const Fact* landingHeading (void) const { return _landingHeading(); }
const Fact* useLoiterToAlt (void) const { return _useLoiterToAlt(); }
const Fact* stopTakingPhotos (void) const { return _stopTakingPhotos(); }
const Fact* stopTakingVideo (void) const { return _stopTakingVideo(); }
Fact* finalApproachAltitude (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_finalApproachAltitude()); };
Fact* loiterRadius (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_loiterRadius()); };
Fact* loiterClockwise (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_loiterClockwise()); };
Fact* landingAltitude (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_landingAltitude()); };
Fact* landingDistance (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_landingDistance()); };
Fact* landingHeading (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_landingHeading()); };
Fact* useLoiterToAlt (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_useLoiterToAlt()); };
Fact* stopTakingPhotos (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_stopTakingPhotos()); };
Fact* stopTakingVideo (void) { return const_cast<Fact*>(const_cast<const LandingComplexItem*>(this)->_stopTakingVideo()); };
bool altitudesAreRelative (void) const { return _altitudesAreRelative; }
bool landingCoordSet (void) const { return _landingCoordSet; }
QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; }
QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; }
QGeoCoordinate finalApproachCoordinate (void) const { return _finalApproachCoordinate; }
QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; }
void setLandingCoordinate (const QGeoCoordinate& coordinate);
void setLoiterCoordinate (const QGeoCoordinate& coordinate);
void setFinalApproachCoordinate (const QGeoCoordinate& coordinate);
void setAltitudesAreRelative (bool altitudesAreRelative);
// Overrides from ComplexMissionItem
double complexDistance(void) const final;
double complexDistance (void) const final;
double greatestDistanceTo (const QGeoCoordinate &other) const final;
int lastSequenceNumber (void) const final;
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final { return true; }
bool specifiesAltitudeOnly (void) const final { return false; }
QString commandDescription (void) const final { return "Landing Pattern"; }
QString commandName (void) const final { return "Landing Pattern"; }
QString abbreviation (void) const final { return "L"; }
QGeoCoordinate coordinate (void) const final { return _finalApproachCoordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double additionalTimeDelay (void) const final { return 0; }
ReadyForSaveState readyForSaveState (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { setFinalApproachCoordinate(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
double amslEntryAlt (void) const final;
double amslExitAlt (void) const final;
double minAMSLAltitude (void) const final { return amslExitAlt(); }
double maxAMSLAltitude (void) const final { return amslEntryAlt(); }
static const char* finalApproachToLandDistanceName;
static const char* finalApproachAltitudeName;
static const char* loiterRadiusName;
static const char* loiterClockwiseName;
static const char* landingHeadingName;
static const char* landingAltitudeName;
static const char* useLoiterToAltName;
static const char* stopTakingPhotosName;
static const char* stopTakingVideoName;
signals:
void loiterCoordinateChanged (QGeoCoordinate coordinate);
void finalApproachCoordinateChanged (QGeoCoordinate coordinate);
void loiterTangentCoordinateChanged (QGeoCoordinate coordinate);
void landingCoordinateChanged (QGeoCoordinate coordinate);
void landingCoordSetChanged (bool landingCoordSet);
void loiterClockwiseChanged (bool loiterClockwise);
void useLoiterToAltChanged (bool useLoiterToAlt);
void altitudesAreRelativeChanged (bool altitudesAreRelative);
void _updateFlightPathSegmentsSignal(void);
......@@ -74,23 +131,61 @@ protected slots:
void _updateFlightPathSegmentsDontCallDirectly (void);
void _recalcFromHeadingAndDistanceChange (void);
void _recalcFromCoordinateChange (void);
void _setDirty (void);
protected:
virtual void _calcGlideSlope(void) = 0;
void _init (void);
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
int _sequenceNumber = 0;
bool _dirty = false;
QGeoCoordinate _loiterCoordinate;
virtual const Fact* _finalApproachAltitude (void) const = 0;
virtual const Fact* _loiterRadius (void) const = 0;
virtual const Fact* _loiterClockwise (void) const = 0;
virtual const Fact* _landingAltitude (void) const = 0;
virtual const Fact* _landingDistance (void) const = 0;
virtual const Fact* _landingHeading (void) const = 0;
virtual const Fact* _useLoiterToAlt (void) const = 0;
virtual const Fact* _stopTakingPhotos (void) const = 0;
virtual const Fact* _stopTakingVideo (void) const = 0;
virtual void _calcGlideSlope (void) = 0;
virtual MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) = 0;
void _init (void);
QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle);
MissionItem* _createDoLandStartItem (int seqNum, QObject* parent);
MissionItem* _createFinalApproachItem(int seqNum, QObject* parent);
QJsonObject _save (void);
bool _load (const QJsonObject& complexObject, int sequenceNumber, const QString& jsonComplexItemTypeValue, bool useDeprecatedRelAltKeys, QString& errorString);
typedef bool (*IsLandItemFunc)(const MissionItem& missionItem);
typedef LandingComplexItem* (*CreateItemFunc)(PlanMasterController* masterController, bool flyView, QObject* parent);
static bool _scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController, IsLandItemFunc isLandItemFunc, CreateItemFunc createItemFunc);
int _sequenceNumber = 0;
bool _dirty = false;
QGeoCoordinate _finalApproachCoordinate;
QGeoCoordinate _loiterTangentCoordinate;
QGeoCoordinate _landingCoordinate;
bool _landingCoordSet = false;
bool _ignoreRecalcSignals = false;
bool _loiterClockwise = true;
bool _altitudesAreRelative = true;
bool _landingCoordSet = false;
bool _ignoreRecalcSignals = false;
bool _altitudesAreRelative = true;
static const char* _jsonFinalApproachCoordinateKey;
static const char* _jsonLoiterRadiusKey;
static const char* _jsonLoiterClockwiseKey;
static const char* _jsonLandingCoordinateKey;
static const char* _jsonAltitudesAreRelativeKey;
static const char* _jsonUseLoiterToAltKey;
static const char* _jsonStopTakingPhotosKey;
static const char* _jsonStopTakingVideoKey;
// Only in older file formats
static const char* _jsonDeprecatedLandingAltitudeRelativeKey;
static const char* _jsonDeprecatedLoiterAltitudeRelativeKey;
static const char* _jsonDeprecatedLoiterCoordinateKey;
private slots:
void _recalcFromRadiusChange(void);
void _recalcFromRadiusChange (void);
void _signalLastSequenceNumberChanged (void);
void _updateFinalApproachCoodinateAltitudeFromFact (void);
void _updateLandingCoodinateAltitudeFromFact (void);
friend class LandingComplexItemTest;
};
src/MissionManager/LandingComplexItemTest.cc 0 → 100644
View file @ 48ffa39e
/****************************************************************************
*
* (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 "LandingComplexItemTest.h"
#include "QGCApplication.h"
#include "MissionCommandTree.h"
#include "MissionCommandUIInfo.h"
#include "CameraSectionTest.h"
#include "JsonHelper.h"
const char* SimpleLandingComplexItem::settingsGroup = "SimpleLandingComplexItemUnitTest";
const char* SimpleLandingComplexItem::jsonComplexItemTypeValue = "utSimpleLandingPattern";
LandingComplexItemTest::LandingComplexItemTest(void)
{
}
void LandingComplexItemTest::init(void)
{
VisualMissionItemTest::init();
_item = new SimpleLandingComplexItem(_masterController, false /* flyView */, this);
// Start in a clean state
QVERIFY(!_item->dirty());
_item->setLandingCoordinate(QGeoCoordinate(47, -122));
_item->setDirty(false);
QVERIFY(!_item->dirty());
VisualMissionItemTest::_createSpy(_item, &_viMultiSpy);
rgSignals[finalApproachCoordinateChangedIndex] = SIGNAL(finalApproachCoordinateChanged(QGeoCoordinate));
rgSignals[loiterTangentCoordinateChangedIndex] = SIGNAL(loiterTangentCoordinateChanged(QGeoCoordinate));
rgSignals[landingCoordinateChangedIndex] = SIGNAL(landingCoordinateChanged(QGeoCoordinate));
rgSignals[landingCoordSetChangedIndex] = SIGNAL(landingCoordSetChanged(bool));
rgSignals[loiterClockwiseChangedIndex] = SIGNAL(loiterClockwiseChanged(bool));
rgSignals[useLoiterToAltChangedIndex] = SIGNAL(useLoiterToAltChanged(bool));
rgSignals[altitudesAreRelativeChangedIndex] = SIGNAL(altitudesAreRelativeChanged(bool));
rgSignals[_updateFlightPathSegmentsSignalIndex] = SIGNAL(_updateFlightPathSegmentsSignal());
_multiSpy = new MultiSignalSpy();
QCOMPARE(_multiSpy->init(_item, rgSignals, cSignals), true);
_validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_masterController, this);
_validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_masterController, this);
_validStopTimeItem = CameraSectionTest::createValidStopTimeItem(_masterController, this);
}
void LandingComplexItemTest::cleanup(void)
{
delete _multiSpy;
delete _validStopVideoItem;
delete _validStopDistanceItem;
delete _validStopTimeItem;
VisualMissionItemTest::cleanup();
}
void LandingComplexItemTest::_testDirty(void)
{
QVERIFY(!_item->dirty());
_item->setDirty(true);
QVERIFY(_item->dirty());
QVERIFY(_viMultiSpy->checkOnlySignalByMask(dirtyChangedMask));
QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_item->setDirty(false);
_viMultiSpy->clearAllSignals();
// These facts should set dirty when changed
QList<Fact*> rgFacts;
rgFacts << _item->finalApproachAltitude()
<< _item->landingHeading()
<< _item->loiterRadius()
<< _item->loiterClockwise()
<< _item->landingAltitude()
<< _item->landingDistance()
<< _item->useLoiterToAlt()
<< _item->stopTakingPhotos()
<< _item->stopTakingVideo();
for(Fact* fact: rgFacts) {
qDebug() << fact->name();
QVERIFY(!_item->dirty());
changeFactValue(fact);
QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_item->setDirty(false);
_viMultiSpy->clearAllSignals();
}
// These bool properties should set dirty when changed
QList<const char*> rgBoolNames;
rgBoolNames << "altitudesAreRelative";
const QMetaObject* metaObject = _item->metaObject();
for(const char* boolName: rgBoolNames) {
qDebug() << boolName;
QVERIFY(!_item->dirty());
QMetaProperty boolProp = metaObject->property(metaObject->indexOfProperty(boolName));
QVERIFY(boolProp.write(_item, !boolProp.read(_item).toBool()));
QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_item->setDirty(false);
_viMultiSpy->clearAllSignals();
}
// These coordinates should set dirty when changed
QVERIFY(!_item->dirty());
_item->setFinalApproachCoordinate(changeCoordinateValue(_item->finalApproachCoordinate()));
QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_item->setDirty(false);
_viMultiSpy->clearAllSignals();
QVERIFY(!_item->dirty());
_item->setLandingCoordinate(changeCoordinateValue(_item->landingCoordinate()));
QVERIFY(_viMultiSpy->checkSignalByMask(dirtyChangedMask));
QVERIFY(_viMultiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
_item->setDirty(false);
_viMultiSpy->clearAllSignals();
}
void LandingComplexItemTest::_testItemCount(void)
{
QList<MissionItem*> items;
struct TestCase_s {
bool stopTakingPhotos;
bool stopTakingVideo;
} rgTestCases[] = {
{ false, false },
{ false, true },
{ true, false },
{ true, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
TestCase_s& testCase = rgTestCases[i];
_item->stopTakingPhotos()->setRawValue(testCase.stopTakingPhotos);
_item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo);
_item->appendMissionItems(items, this);
QCOMPARE(items.count(), 3 + (testCase.stopTakingPhotos * CameraSection::stopTakingPhotosCommandCount()) + (testCase.stopTakingVideo * CameraSection::stopTakingVideoCommandCount()));
QCOMPARE(items.count() - 1, _item->lastSequenceNumber());
items.clear();
}
}
void LandingComplexItemTest::_testAppendSectionItems(void)
{
QList<MissionItem*> rgMissionItems;
struct TestCase_s {
bool stopTakingPhotos;
bool stopTakingVideo;
bool useLoiterToAlt;
} rgTestCases[] = {
{ false, false, false },
{ false, false, true },
{ false, true, false },
{ false, true, true },
{ true, false, false },
{ true, false, true },
{ true, true, false },
{ true, true, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
TestCase_s& testCase = rgTestCases[i];
qDebug() << "stopTakingPhotos" << testCase.stopTakingPhotos << "stopTakingVideo" << testCase.stopTakingVideo << "useLoiterToAlt" << testCase.useLoiterToAlt;
_item->stopTakingPhotos()->setRawValue(testCase.stopTakingPhotos);
_item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo);
_item->useLoiterToAlt()->setRawValue(testCase.useLoiterToAlt);
_item->appendMissionItems(rgMissionItems, this);
// First item should be DO_LAND_START always
QCOMPARE(rgMissionItems[0]->command(), MAV_CMD_DO_LAND_START);
// Next come stop photo/video
// Convert to simple visual items for verification
QmlObjectListModel* simpleItems = new QmlObjectListModel(this);
for (MissionItem* item: rgMissionItems) {
SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, simpleItems);
simpleItem->missionItem() = *item;
simpleItems->append(simpleItem);
}
// Validate stop commands
QCOMPARE(CameraSection::scanStopTakingPhotos(simpleItems, 1, false /* removeScannedItems */), testCase.stopTakingPhotos);
QCOMPARE(CameraSection::scanStopTakingVideo(simpleItems, 1 + (testCase.stopTakingPhotos ? CameraSection::stopTakingPhotosCommandCount() : 0), false /* removeScannedItems */), testCase.stopTakingVideo);
// Lastly is final approach item and land
int finalApproachIndex = 1 + (testCase.stopTakingPhotos ? CameraSection::stopTakingPhotosCommandCount() : 0) + (testCase.stopTakingVideo ? CameraSection::stopTakingVideoCommandCount() : 0);
QCOMPARE(rgMissionItems[finalApproachIndex]->command(), testCase.useLoiterToAlt ? MAV_CMD_NAV_LOITER_TO_ALT : MAV_CMD_NAV_WAYPOINT);
qDebug() << rgMissionItems[finalApproachIndex+1]->command();
QCOMPARE(rgMissionItems[finalApproachIndex+1]->command(), MAV_CMD_NAV_LAND);
simpleItems->deleteLater();
rgMissionItems.clear();
}
}
void LandingComplexItemTest::_testScanForItems(void)
{
QList<MissionItem*> rgMissionItems;
struct TestCase_s {
bool stopTakingPhotos;
bool stopTakingVideo;
bool useLoiterToAlt;
} rgTestCases[] = {
{ false, false, false },
{ false, false, true },
{ false, true, false },
{ false, true, true },
{ true, false, false },
{ true, false, true },
{ true, true, false },
{ true, true, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
TestCase_s& testCase = rgTestCases[i];
qDebug() << "stopTakingPhotos" << testCase.stopTakingPhotos << "stopTakingVideo" << testCase.stopTakingVideo << "useLoiterToAlt" << testCase.useLoiterToAlt;
_item->stopTakingPhotos()->setRawValue(testCase.stopTakingPhotos);
_item->stopTakingVideo()->setRawValue(testCase.stopTakingVideo);
_item->useLoiterToAlt()->setRawValue(testCase.useLoiterToAlt);
_item->appendMissionItems(rgMissionItems, this);
// Convert to simple visual items for _scan
QmlObjectListModel* visualItems = new QmlObjectListModel(this);
for (MissionItem* item: rgMissionItems) {
SimpleMissionItem* simpleItem = new SimpleMissionItem(_masterController, false /* flyView */, false /* forLoad */, visualItems);
simpleItem->missionItem() = *item;
visualItems->append(simpleItem);
}
QVERIFY(LandingComplexItem::_scanForItem(visualItems, false /* flyView */, _masterController, &SimpleLandingComplexItem::_isValidLandItem, &SimpleLandingComplexItem::_createItem));
QCOMPARE(visualItems->count(), 1);
SimpleLandingComplexItem* scannedItem = visualItems->value<SimpleLandingComplexItem*>(0);
QVERIFY(scannedItem);
_validateItem(scannedItem, _item);
visualItems->deleteLater();
rgMissionItems.clear();
}
}
void LandingComplexItemTest::_testSaveLoad(void)
{
QString errorString;
QJsonObject saveObject = _item->_save();
saveObject[JsonHelper::jsonVersionKey] = 1;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = SimpleLandingComplexItem::jsonComplexItemTypeValue;
// Test useDeprecatedRelAltKeys = false
SimpleLandingComplexItem* newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */);
bool loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString);
if (!loadSuccess) {
qDebug() << "_load failed" << errorString;
}
QVERIFY(loadSuccess);
QVERIFY(errorString.isEmpty());
_validateItem(newItem, _item);
newItem->deleteLater();
// Test useDeprecatedRelAltKeys = true
bool relAlt = saveObject[LandingComplexItem::_jsonAltitudesAreRelativeKey].toBool();
saveObject[LandingComplexItem::_jsonDeprecatedLoiterAltitudeRelativeKey] = relAlt;
saveObject[LandingComplexItem::_jsonDeprecatedLandingAltitudeRelativeKey] = relAlt;
saveObject.remove(LandingComplexItem::_jsonAltitudesAreRelativeKey);
newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */);
loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, true /* useDeprecatedRelAltKeys */, errorString);
if (!loadSuccess) {
qDebug() << "_load failed" << errorString;
}
QVERIFY(loadSuccess);
QVERIFY(errorString.isEmpty());
_validateItem(newItem, _item);
newItem->deleteLater();
// Test for _jsonDeprecatedLoiterCoordinateKey support
saveObject = _item->_save();
saveObject[JsonHelper::jsonVersionKey] = 1;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = SimpleLandingComplexItem::jsonComplexItemTypeValue;
saveObject[LandingComplexItem::_jsonDeprecatedLoiterCoordinateKey] = saveObject[LandingComplexItem::_jsonFinalApproachCoordinateKey];
saveObject.remove(LandingComplexItem::_jsonFinalApproachCoordinateKey);
newItem = new SimpleLandingComplexItem(_masterController, false /* flyView */, this /* parent */);
loadSuccess = newItem->_load(saveObject, 0 /* sequenceNumber */, SimpleLandingComplexItem::jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString);
if (!loadSuccess) {
qDebug() << "_load failed" << errorString;
}
QVERIFY(loadSuccess);
QVERIFY(errorString.isEmpty());
_validateItem(newItem, _item);
newItem->deleteLater();
}
void LandingComplexItemTest::_validateItem(LandingComplexItem* actualItem, LandingComplexItem* expectedItem)
{
QVERIFY(actualItem);
QCOMPARE(actualItem->stopTakingPhotos()->rawValue().toBool(), expectedItem->stopTakingPhotos()->rawValue().toBool());
QCOMPARE(actualItem->stopTakingVideo()->rawValue().toBool(), expectedItem->stopTakingVideo()->rawValue().toBool());
QCOMPARE(actualItem->useLoiterToAlt()->rawValue().toBool(), expectedItem->useLoiterToAlt()->rawValue().toBool());
QCOMPARE(actualItem->finalApproachAltitude()->rawValue().toInt(), expectedItem->finalApproachAltitude()->rawValue().toInt());
QCOMPARE(actualItem->landingAltitude()->rawValue().toInt(), expectedItem->landingAltitude()->rawValue().toInt());
QCOMPARE(actualItem->landingHeading()->rawValue().toInt(), expectedItem->landingHeading()->rawValue().toInt());
QCOMPARE(actualItem->landingDistance()->rawValue().toInt(), expectedItem->landingDistance()->rawValue().toInt());
QCOMPARE(actualItem->altitudesAreRelative(), expectedItem->altitudesAreRelative());
QCOMPARE(actualItem->landingCoordSet(), expectedItem->landingCoordSet());
QVERIFY(fuzzyCompareLatLon(actualItem->finalApproachCoordinate(), expectedItem->finalApproachCoordinate()));
QVERIFY(fuzzyCompareLatLon(actualItem->landingCoordinate(), expectedItem->landingCoordinate()));
if (actualItem->useLoiterToAlt()->rawValue().toBool()) {
QVERIFY(fuzzyCompareLatLon(actualItem->loiterTangentCoordinate(), expectedItem->loiterTangentCoordinate()));
QCOMPARE(actualItem->loiterRadius()->rawValue().toInt(), expectedItem->loiterRadius()->rawValue().toInt());
QCOMPARE(actualItem->loiterClockwise()->rawValue().toBool(), expectedItem->loiterClockwise()->rawValue().toBool());
}
}
SimpleLandingComplexItem::SimpleLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
: LandingComplexItem (masterController, flyView, parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this))
, _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
, _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
, _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName])
, _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName])
, _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName])
, _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName])
, _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName])
, _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName])
, _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName])
{
_isIncomplete = false;
_init();
_recalcFromHeadingAndDistanceChange();
setDirty(false);
}
MissionItem* SimpleLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
{
return new MissionItem(seqNum,
MAV_CMD_NAV_LAND,
altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
0.0, 0.0, 0.0, 0.0,
lat, lon, alt,
true, // autoContinue
false, // isCurrentItem
parent);
}
bool SimpleLandingComplexItem::_isValidLandItem(const MissionItem& missionItem)
{
if (missionItem.command() != MAV_CMD_NAV_LAND ||
!(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) ||
missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0 ||
qIsNaN(missionItem.param5()) || qIsNaN(missionItem.param6()) || qIsNaN(missionItem.param7())) {
return false;
} else {
return true;
}
}
src/MissionManager/LandingComplexItemTest.h 0 → 100644
View file @ 48ffa39e
/****************************************************************************
*
* (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 "VisualMissionItemTest.h"
#include "LandingComplexItem.h"
#include "MultiSignalSpy.h"
#include "PlanMasterController.h"
class LandingComplexItemTest : public VisualMissionItemTest
{
Q_OBJECT
public:
LandingComplexItemTest(void);
void cleanup(void) override;
void init (void) override;
private slots:
void _testDirty (void);
void _testItemCount (void);
void _testAppendSectionItems (void);
void _testScanForItems (void);
void _testSaveLoad (void);
private:
void _validateItem(LandingComplexItem* actualItem, LandingComplexItem* expectedItem);
enum {
finalApproachCoordinateChangedIndex = 0,
loiterTangentCoordinateChangedIndex,
landingCoordinateChangedIndex,
landingCoordSetChangedIndex,
loiterClockwiseChangedIndex,
useLoiterToAltChangedIndex,
altitudesAreRelativeChangedIndex,
_updateFlightPathSegmentsSignalIndex,
maxSignalIndex,
};
enum {
finalApproachCoordinateChangedMask = 1 << finalApproachCoordinateChangedIndex,
loiterTangentCoordinateChangedMask = 1 << loiterTangentCoordinateChangedIndex,
landingCoordinateChangedMask = 1 << landingCoordinateChangedIndex,
landingCoordSetChangedMask = 1 << landingCoordSetChangedIndex,
loiterClockwiseChangedMask = 1 << loiterClockwiseChangedIndex,
useLoiterToAltChangedMask = 1 << useLoiterToAltChangedIndex,
altitudesAreRelativeChangedMask = 1 << altitudesAreRelativeChangedIndex,
_updateFlightPathSegmentsSignalMask = 1 << _updateFlightPathSegmentsSignalIndex,
};
static const size_t cSignals = maxSignalIndex;
const char* rgSignals[cSignals];
LandingComplexItem* _item = nullptr;
MultiSignalSpy* _multiSpy = nullptr;
MultiSignalSpy* _viMultiSpy = nullptr;
SimpleMissionItem* _validStopVideoItem = nullptr;
SimpleMissionItem* _validStopDistanceItem = nullptr;
SimpleMissionItem* _validStopTimeItem = nullptr;
};
// Simple class used to test LandingComplexItem base class
class SimpleLandingComplexItem : public LandingComplexItem
{
Q_OBJECT
public:
SimpleLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent);
// Overrides from ComplexMissionItem
QString patternName (void) const final { return QString(); }
bool load (const QJsonObject& /*complexObject*/, int /*sequenceNumber*/, QString& /*errorString*/) final { return false; };
QString mapVisualQML(void) const final { return QStringLiteral("FWLandingPatternMapVisual.qml"); }
// Overrides from VisualMissionItem
void save (QJsonArray& /*missionItems*/) { };
static const QString name;
static const char* jsonComplexItemTypeValue;
static const char* settingsGroup;
private:
static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new SimpleLandingComplexItem(masterController, flyView, parent); }
static bool _isValidLandItem(const MissionItem& missionItem);
// Overrides from LandingComplexItem
const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; }
const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; }
const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; }
const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; }
const Fact* _landingDistance (void) const final { return &_landingDistanceFact; }
const Fact* _landingHeading (void) const final { return &_landingHeadingFact; }
const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; }
const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; }
const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; }
void _calcGlideSlope (void) final { };
MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final;
QMap<QString, FactMetaData*> _metaDataMap;
Fact _landingDistanceFact;
Fact _finalApproachAltitudeFact;
Fact _loiterRadiusFact;
Fact _loiterClockwiseFact;
Fact _landingHeadingFact;
Fact _landingAltitudeFact;
Fact _useLoiterToAltFact;
Fact _stopTakingPhotosFact;
Fact _stopTakingVideoFact;
friend class LandingComplexItemTest;
};
src/MissionManager/MissionController.cc
View file @ 48ffa39e
......@@ -2153,8 +2153,10 @@ void MissionController::setDirty(bool dirty)
void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, PlanMasterController* masterController)
{
// First we look for a Fixed Wing Landing Pattern which is at the end
FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController);
// First we look for a Landing Patterns which are at the end
if (!FixedWingLandingComplexItem::scanForItem(visualItems, _flyView, masterController)) {
VTOLLandingComplexItem::scanForItem(visualItems, _flyView, masterController);
}
int scanIndex = 0;
while (scanIndex < visualItems->count()) {
......
src/MissionManager/VTOLLandingComplexItem.cc
View file @ 48ffa39e
......@@ -25,35 +25,21 @@ const QString VTOLLandingComplexItem::name(tr("VTOL Landing"));
const char* VTOLLandingComplexItem::settingsGroup = "VTOLLanding";
const char* VTOLLandingComplexItem::jsonComplexItemTypeValue = "vtolLandingPattern";
const char* VTOLLandingComplexItem::loiterToLandDistanceName = "LandingDistance";
const char* VTOLLandingComplexItem::landingHeadingName = "LandingHeading";
const char* VTOLLandingComplexItem::loiterAltitudeName = "LoiterAltitude";
const char* VTOLLandingComplexItem::loiterRadiusName = "LoiterRadius";
const char* VTOLLandingComplexItem::landingAltitudeName = "LandingAltitude";
const char* VTOLLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos";
const char* VTOLLandingComplexItem::stopTakingVideoName = "StopTakingVideo";
const char* VTOLLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate";
const char* VTOLLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius";
const char* VTOLLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise";
const char* VTOLLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate";
const char* VTOLLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative";
const char* VTOLLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos";
const char* VTOLLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos";
VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent)
: LandingComplexItem (masterController, flyView, parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/VTOLLandingPattern.FactMetaData.json"), this))
, _landingDistanceFact (settingsGroup, _metaDataMap[loiterToLandDistanceName])
, _loiterAltitudeFact (settingsGroup, _metaDataMap[loiterAltitudeName])
, _landingDistanceFact (settingsGroup, _metaDataMap[finalApproachToLandDistanceName])
, _finalApproachAltitudeFact(settingsGroup, _metaDataMap[finalApproachAltitudeName])
, _loiterRadiusFact (settingsGroup, _metaDataMap[loiterRadiusName])
, _loiterClockwiseFact (settingsGroup, _metaDataMap[loiterClockwiseName])
, _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName])
, _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName])
, _useLoiterToAltFact (settingsGroup, _metaDataMap[useLoiterToAltName])
, _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName])
, _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName])
{
_editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml";
_isIncomplete = false;
_editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml";
_isIncomplete = false;
_init();
......@@ -66,237 +52,42 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr
_landingDistanceFact.metaData()->setRawMin(vtolTransitionDistanceFact->metaData()->rawMin());
}
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged);
connect(this, &VTOLLandingComplexItem::amslEntryAltChanged, this, &VTOLLandingComplexItem::maxAMSLAltitudeChanged);
connect(this, &VTOLLandingComplexItem::amslExitAltChanged, this, &VTOLLandingComplexItem::minAMSLAltitudeChanged);
connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged);
connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_signalLastSequenceNumberChanged);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_landingDistanceFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_loiterRadiusFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(this, &VTOLLandingComplexItem::loiterClockwiseChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_setDirty);
connect(this, &VTOLLandingComplexItem::landingCoordSetChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged);
connect(this, &VTOLLandingComplexItem::wizardModeChanged, this, &VTOLLandingComplexItem::readyForSaveStateChanged);
connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::complexDistanceChanged);
connect(this, &VTOLLandingComplexItem::loiterTangentCoordinateChanged,this, &VTOLLandingComplexItem::complexDistanceChanged);
connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::complexDistanceChanged);
connect(this, &VTOLLandingComplexItem::loiterTangentCoordinateChanged,this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal);
// The follow is used to compress multiple recalc calls in a row to into a single call.
connect(this, &VTOLLandingComplexItem::_updateFlightPathSegmentsSignal, this, &VTOLLandingComplexItem::_updateFlightPathSegmentsDontCallDirectly, Qt::QueuedConnection);
qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&VTOLLandingComplexItem::_updateFlightPathSegmentsSignal));
if (_masterController->controllerVehicle()->apmFirmware()) {
// ArduPilot does not support camera commands
_stopTakingVideoFact.setRawValue(false);
_stopTakingPhotosFact.setRawValue(false);
}
_recalcFromHeadingAndDistanceChange();
setDirty(false);
}
int VTOLLandingComplexItem::lastSequenceNumber(void) const
{
// Fixed items are:
// land start, loiter, land
// Optional items are:
// stop photos/video
return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0);
}
void VTOLLandingComplexItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
_dirty = dirty;
emit dirtyChanged(_dirty);
}
}
void VTOLLandingComplexItem::save(QJsonArray& missionItems)
{
QJsonObject saveObject;
QJsonObject saveObject = _save();
saveObject[JsonHelper::jsonVersionKey] = 1;
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;
QGeoCoordinate coordinate;
QJsonValue jsonCoordinate;
coordinate = _loiterCoordinate;
coordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonLoiterCoordinateKey] = jsonCoordinate;
coordinate = _landingCoordinate;
coordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
JsonHelper::saveGeoCoordinate(coordinate, true /* writeAltitude */, jsonCoordinate);
saveObject[_jsonLandingCoordinateKey] = jsonCoordinate;
saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble();
saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool();
saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool();
saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise;
saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative;
missionItems.append(saveObject);
}
void VTOLLandingComplexItem::setSequenceNumber(int sequenceNumber)
{
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(sequenceNumber);
emit lastSequenceNumberChanged(lastSequenceNumber());
}
}
bool VTOLLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true },
{ _jsonLoiterCoordinateKey, QJsonValue::Array, true },
{ _jsonLoiterRadiusKey, QJsonValue::Double, true },
{ _jsonLoiterClockwiseKey, QJsonValue::Bool, true },
{ _jsonLandingCoordinateKey, QJsonValue::Array, true },
{ _jsonStopTakingPhotosKey, QJsonValue::Bool, false },
{ _jsonStopTakingVideoKey, QJsonValue::Bool, false },
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}
QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
errorString = tr("%1 does not support loading this complex mission item type: %2:%3").arg(qgcApp()->applicationName()).arg(itemType).arg(complexType);
return false;
}
setSequenceNumber(sequenceNumber);
_ignoreRecalcSignals = true;
int version = complexObject[JsonHelper::jsonVersionKey].toInt();
if (version == 1) {
QList<JsonHelper::KeyValidateInfo> v2KeyInfoList = {
{ _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, v2KeyInfoList, errorString)) {
_ignoreRecalcSignals = false;
return false;
}
_altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool();
} else {
if (version != 1) {
errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version);
_ignoreRecalcSignals = false;
return false;
}
QGeoCoordinate coordinate;
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_loiterCoordinate = coordinate;
_loiterAltitudeFact.setRawValue(coordinate.altitude());
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_landingCoordinate = coordinate;
_landingAltitudeFact.setRawValue(coordinate.altitude());
_loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
_loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool();
if (complexObject.contains(_jsonStopTakingPhotosKey)) {
_stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool());
} else {
_stopTakingPhotosFact.setRawValue(false);
}
if (complexObject.contains(_jsonStopTakingVideoKey)) {
_stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool());
} else {
_stopTakingVideoFact.setRawValue(false);
}
_landingCoordSet = true;
_ignoreRecalcSignals = false;
_recalcFromCoordinateChange();
emit coordinateChanged(this->coordinate()); // This will kick off terrain query
return true;
}
double VTOLLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
}
bool VTOLLandingComplexItem::specifiesCoordinate(void) const
{
return true;
}
MissionItem* VTOLLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent)
{
return new MissionItem(seqNum, // sequence number
MAV_CMD_DO_LAND_START, // MAV_CMD
MAV_FRAME_MISSION, // MAV_FRAME
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7
true, // autoContinue
false, // isCurrentItem
parent);
}
MissionItem* VTOLLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent)
{
return new MissionItem(seqNum,
MAV_CMD_NAV_LOITER_TO_ALT,
altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
1.0, // Heading required = true
loiterRadius, // Loiter radius
0.0, // param 3 - unused
1.0, // Exit crosstrack - tangent of loiter to land point
lat, lon, alt,
true, // autoContinue
false, // isCurrentItem
parent);
return _load(complexObject, sequenceNumber, jsonComplexItemTypeValue, false /* useDeprecatedRelAltKeys */, errorString);
}
MissionItem* VTOLLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
MissionItem* VTOLLandingComplexItem::_createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent)
{
return new MissionItem(seqNum,
MAV_CMD_NAV_VTOL_LAND,
......@@ -309,219 +100,23 @@ MissionItem* VTOLLandingComplexItem::createLandItem(int seqNum, bool altRel, dou
}
void VTOLLandingComplexItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
void VTOLLandingComplexItem::_calcGlideSlope(void)
{
int seqNum = _sequenceNumber;
// IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem
MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent);
items.append(item);
if (_stopTakingPhotosFact.rawValue().toBool()) {
CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent);
}
if (_stopTakingVideoFact.rawValue().toBool()) {
CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent);
}
double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0);
item = createLoiterToAltItem(seqNum++,
_altitudesAreRelative,
loiterRadius,
_loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(),
missionItemParent);
items.append(item);
item = createLandItem(seqNum++,
_altitudesAreRelative,
_landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(),
missionItemParent);
items.append(item);
// No glide slope calc for VTOL
}
bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
bool VTOLLandingComplexItem::_isValidLandItem(const MissionItem& missionItem)
{
qCDebug(VTOLLandingComplexItemLog) << "VTOLLandingComplexItem::scanForItem count" << visualItems->count();
if (visualItems->count() < 3) {
if (missionItem.command() != MAV_CMD_NAV_LAND ||
!(missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItem.frame() == MAV_FRAME_GLOBAL) ||
missionItem.param1() != 0 || missionItem.param2() != 0 || missionItem.param3() != 0 || missionItem.param4() != 0) {
return false;
} else {
return true;
}
// A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list:
// MAV_CMD_DO_LAND_START - required
// Stop taking photos sequence - optional
// Stop taking video sequence - optional
// MAV_CMD_NAV_LOITER_TO_ALT - required
// MAV_CMD_NAV_LAND - required
// Start looking for the commands in reverse order from the end of the list
int scanIndex = visualItems->count() - 1;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(scanIndex--);
if (!item) {
return false;
}
MissionItem& missionItemLand = item->missionItem();
if (missionItemLand.command() != MAV_CMD_NAV_LAND ||
!(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) ||
missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() != 0) {
return false;
}
MAV_FRAME landPointFrame = missionItemLand.frame();
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
MissionItem& missionItemLoiter = item->missionItem();
if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT ||
missionItemLoiter.frame() != landPointFrame ||
missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) {
return false;
}
scanIndex -= CameraSection::stopTakingVideoCommandCount();
bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingVideo) {
scanIndex += CameraSection::stopTakingVideoCommandCount();
}
scanIndex -= CameraSection::stopTakingPhotosCommandCount();
bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */);
if (!stopTakingPhotos) {
scanIndex += CameraSection::stopTakingPhotosCommandCount();
}
scanIndex--;
if (scanIndex < 0 || scanIndex > visualItems->count() - 1) {
return false;
}
item = visualItems->value<SimpleMissionItem*>(scanIndex);
if (!item) {
return false;
}
MissionItem& missionItemDoLandStart = item->missionItem();
if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
missionItemDoLandStart.frame() != MAV_FRAME_MISSION ||
missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0) {
return false;
}
// We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission.
// Since we have scanned it we need to remove the items for it fromt the list
int deleteCount = 3;
if (stopTakingPhotos) {
deleteCount += CameraSection::stopTakingPhotosCommandCount();
}
if (stopTakingVideo) {
deleteCount += CameraSection::stopTakingVideoCommandCount();
}
int firstItem = visualItems->count() - deleteCount;
while (deleteCount--) {
visualItems->removeAt(firstItem)->deleteLater();
}
// Now stuff all the scanned information into the item
VTOLLandingComplexItem* complexItem = new VTOLLandingComplexItem(masterController, flyView, visualItems);
complexItem->_ignoreRecalcSignals = true;
complexItem->_altitudesAreRelative = landPointFrame == MAV_FRAME_GLOBAL_RELATIVE_ALT;
complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2()));
complexItem->_loiterClockwise = missionItemLoiter.param2() > 0;
complexItem->setLoiterCoordinate(QGeoCoordinate(missionItemLoiter.param5(), missionItemLoiter.param6()));
complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7());
complexItem->_landingCoordinate.setLatitude(missionItemLand.param5());
complexItem->_landingCoordinate.setLongitude(missionItemLand.param6());
complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7());
complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos);
complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo);
complexItem->_landingCoordSet = true;
complexItem->_ignoreRecalcSignals = false;
complexItem->_recalcFromCoordinateChange();
complexItem->setDirty(false);
visualItems->append(complexItem);
return true;
}
double VTOLLandingComplexItem::_mathematicAngleToHeading(double angle)
{
double heading = (angle - 90) * -1;
if (heading < 0) {
heading += 360;
}
return heading;
}
double VTOLLandingComplexItem::_headingToMathematicAngle(double heading)
{
return heading - 90 * -1;
}
void VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
}
void VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
_landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
emit landingCoordinateChanged(_landingCoordinate);
}
void VTOLLandingComplexItem::_setDirty(void)
{
setDirty(true);
}
void VTOLLandingComplexItem::applyNewAltitude(double newAltitude)
{
_loiterAltitudeFact.setRawValue(newAltitude);
}
void VTOLLandingComplexItem::_signalLastSequenceNumberChanged(void)
{
emit lastSequenceNumberChanged(lastSequenceNumber());
}
VTOLLandingComplexItem::ReadyForSaveState VTOLLandingComplexItem::readyForSaveState(void) const
{
return _landingCoordSet && !_wizardMode ? ReadyForSave : NotReadyForSaveData;
}
double VTOLLandingComplexItem::amslEntryAlt(void) const
{
return _loiterAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
}
double VTOLLandingComplexItem::amslExitAlt(void) const
{
return _landingAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
}
void VTOLLandingComplexItem::_calcGlideSlope(void)
bool VTOLLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController)
{
// No glide slope calc for VTOL
return _scanForItem(visualItems, flyView, masterController, _isValidLandItem, _createItem);
}
src/MissionManager/VTOLLandingComplexItem.h
View file @ 48ffa39e
......@@ -26,98 +26,51 @@ class VTOLLandingComplexItem : public LandingComplexItem
public:
VTOLLandingComplexItem(PlanMasterController* masterController, bool flyView, QObject* parent);
Fact* loiterAltitude (void) final { return &_loiterAltitudeFact; }
Fact* loiterRadius (void) final { return &_loiterRadiusFact; }
Fact* landingAltitude (void) final { return &_landingAltitudeFact; }
Fact* landingDistance (void) final { return &_landingDistanceFact; }
Fact* landingHeading (void) final { return &_landingHeadingFact; }
Fact* stopTakingPhotos (void) final { return &_stopTakingPhotosFact; }
Fact* stopTakingVideo (void) final { return &_stopTakingVideoFact; }
/// Scans the loaded items for a landing pattern complex item
static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, PlanMasterController* masterController);
static MissionItem* createDoLandStartItem (int seqNum, QObject* parent);
static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent);
static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent);
// Overrides from ComplexMissionItem
QString patternName (void) const final { return name; }
int lastSequenceNumber (void) const final;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
double greatestDistanceTo (const QGeoCoordinate &other) const final;
QString mapVisualQML (void) const final { return QStringLiteral("VTOLLandingPatternMapVisual.qml"); }
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final;
bool specifiesAltitudeOnly (void) const final { return false; }
QString commandDescription (void) const final { return "Landing Pattern"; }
QString commandName (void) const final { return "Landing Pattern"; }
QString abbreviation (void) const final { return "L"; }
QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
double additionalTimeDelay (void) const final { return 0; }
ReadyForSaveState readyForSaveState (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return false; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonArray& missionItems) final;
double amslEntryAlt (void) const final;
double amslExitAlt (void) const final;
double minAMSLAltitude (void) const final { return amslExitAlt(); }
double maxAMSLAltitude (void) const final { return amslEntryAlt(); }
static const QString name;
static const char* jsonComplexItemTypeValue;
static const char* settingsGroup;
static const char* loiterToLandDistanceName;
static const char* loiterAltitudeName;
static const char* loiterRadiusName;
static const char* landingHeadingName;
static const char* landingAltitudeName;
static const char* stopTakingPhotosName;
static const char* stopTakingVideoName;
private slots:
void _updateLoiterCoodinateAltitudeFromFact (void);
void _updateLandingCoodinateAltitudeFromFact (void);
double _mathematicAngleToHeading (double angle);
double _headingToMathematicAngle (double heading);
void _setDirty (void);
void _signalLastSequenceNumberChanged (void);
private:
void _calcGlideSlope(void) final;
static LandingComplexItem* _createItem (PlanMasterController* masterController, bool flyView, QObject* parent) { return new VTOLLandingComplexItem(masterController, flyView, parent); }
static bool _isValidLandItem(const MissionItem& missionItem);
// Overrides from LandingComplexItem
const Fact* _finalApproachAltitude (void) const final { return &_finalApproachAltitudeFact; }
const Fact* _loiterRadius (void) const final { return &_loiterRadiusFact; }
const Fact* _loiterClockwise (void) const final { return &_loiterClockwiseFact; }
const Fact* _landingAltitude (void) const final { return &_landingAltitudeFact; }
const Fact* _landingDistance (void) const final { return &_landingDistanceFact; }
const Fact* _landingHeading (void) const final { return &_landingHeadingFact; }
const Fact* _useLoiterToAlt (void) const final { return &_useLoiterToAltFact; }
const Fact* _stopTakingPhotos (void) const final { return &_stopTakingPhotosFact; }
const Fact* _stopTakingVideo (void) const final { return &_stopTakingVideoFact; }
void _calcGlideSlope (void) final;
MissionItem* _createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) final;
QMap<QString, FactMetaData*> _metaDataMap;
Fact _landingDistanceFact;
Fact _loiterAltitudeFact;
Fact _finalApproachAltitudeFact;
Fact _loiterRadiusFact;
Fact _loiterClockwiseFact;
Fact _landingHeadingFact;
Fact _landingAltitudeFact;
Fact _useLoiterToAltFact;
Fact _stopTakingPhotosFact;
Fact _stopTakingVideoFact;
static const char* _jsonLoiterCoordinateKey;
static const char* _jsonLoiterRadiusKey;
static const char* _jsonLoiterClockwiseKey;
static const char* _jsonLandingCoordinateKey;
static const char* _jsonAltitudesAreRelativeKey;
static const char* _jsonStopTakingPhotosKey;
static const char* _jsonStopTakingVideoKey;
friend VTOLLandingPatternTest;
};
src/MissionManager/VTOLLandingPattern.FactMetaData.json
View file @ 48ffa39e
......@@ -5,7 +5,7 @@
[
{
"name": "LandingDistance",
"shortDesc": "Distance between landing and loiter points.",
"shortDesc": "Distance between landing and approach points.",
"type": "double",
"units": "m",
"min": 10,
......@@ -14,7 +14,7 @@
},
{
"name": "LandingHeading",
"shortDesc": "Heading from loiter point to land point.",
"shortDesc": "Heading from approach to land point.",
"type": "double",
"units": "deg",
"min": 0.0,
......@@ -23,8 +23,8 @@
"default": 270.0
},
{
"name": "LoiterAltitude",
"shortDesc": "Aircraft will proceed to the loiter point and loiter downwards until it reaches this approach altitude. Once altitude is reached the aircraft will fly to land point at current altitude.",
"name": "FinalApproachAltitude",
"shortDesc": "Altitude to begin landing approach from.",
"type": "double",
"units": "m",
"decimalPlaces": 1,
......@@ -39,6 +39,12 @@
"units": "m",
"default": 75.0
},
{
"name": "LoiterClockwise",
"shortDesc": "Loiter clockwise around the final approach point.",
"type": "bool",
"default": true
},
{
"name": "LandingAltitude",
"shortDesc": "Altitude for landing point on ground.",
......@@ -47,6 +53,12 @@
"decimalPlaces": 1,
"default": 0.0
},
{
"name": "UseLoiterToAlt",
"shortDesc": "Use a loiter to altitude item for final appoach. Otherwise use a regular waypoint.",
"type": "bool",
"default": true
},
{
"name": "StopTakingPhotos",
"shortDesc": "Stop taking photos",
......
src/MissionManager/VisualMissionItemTest.cc
View file @ 48ffa39e
......@@ -55,10 +55,10 @@ void VisualMissionItemTest::cleanup(void)
_masterController->deleteLater();
}
void VisualMissionItemTest::_createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy)
void VisualMissionItemTest::_createSpy(VisualMissionItem* visualItem, MultiSignalSpy** visualSpy)
{
*visualSpy = nullptr;
MultiSignalSpy* spy = new MultiSignalSpy();
QCOMPARE(spy->init(simpleItem, rgVisualItemSignals, cVisualItemSignals), true);
QCOMPARE(spy->init(visualItem, rgVisualItemSignals, cVisualItemSignals), true);
*visualSpy = spy;
}
src/MissionManager/VisualMissionItemTest.h
View file @ 48ffa39e
......@@ -30,7 +30,7 @@ public:
void cleanup(void) override;
protected:
void _createSpy(SimpleMissionItem* simpleItem, MultiSignalSpy** visualSpy);
void _createSpy(VisualMissionItem* visualItem, MultiSignalSpy** visualSpy);
enum {
altDifferenceChangedIndex = 0,
......
src/PlanView/FWLandingPatternEditor.qml
View file @ 48ffa39e
......@@ -52,20 +52,26 @@ Rectangle {
visible: !editorColumnNeedLandingPoint.visible
SectionHeader {
id: loiterPointSection
id: finalApproachSection
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("Loiter point")
text: qsTr("Final approach")
}
Column {
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
visible: loiterPointSection.checked
visible: finalApproachSection.checked
Item { width: 1; height: _spacer }
FactCheckBox {
text: qsTr("Use loiter to altitude")
fact: missionItem.useLoiterToAlt
visible: missionItem.useLoiterToAlt.visible
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
......@@ -75,24 +81,28 @@ Rectangle {
AltitudeFactTextField {
Layout.fillWidth: true
fact: missionItem.loiterAltitude
fact: missionItem.finalApproachAltitude
altitudeMode: _altitudeMode
}
QGCLabel { text: qsTr("Radius") }
QGCLabel {
text: qsTr("Radius")
visible: missionItem.useLoiterToAlt.rawValue
}
FactTextField {
Layout.fillWidth: true
fact: missionItem.loiterRadius
visible: missionItem.useLoiterToAlt.rawValue
}
}
Item { width: 1; height: _spacer }
QGCCheckBox {
text: qsTr("Loiter clockwise")
checked: missionItem.loiterClockwise
onClicked: missionItem.loiterClockwise = checked
FactCheckBox {
text: qsTr("Loiter clockwise")
fact: missionItem.loiterClockwise
visible: missionItem.useLoiterToAlt.rawValue
}
QGCButton {
......@@ -139,7 +149,7 @@ Rectangle {
QGCRadioButton {
id: specifyLandingDistance
text: qsTr("Landing Dist")
text: qsTr("Distance")
checked: missionItem.valueSetIsDistance.rawValue
onClicked: missionItem.valueSetIsDistance.rawValue = checked
Layout.fillWidth: true
......
src/PlanView/FWLandingPatternMapVisual.qml
View file @ 48ffa39e
......@@ -31,21 +31,27 @@ Item {
readonly property real _landingWidthMeters: 15
readonly property real _landingLengthMeters: 100
property var _missionItem: object
property var _missionItem: object
property var _mouseArea
property var _dragAreas: [ ]
property var _dragAreas: [ ]
property var _flightPath
property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate)
property var _loiterPointObject
property var _landingPointObject
property real _transitionAltitudeMeters
property real _midSlopeAltitudeMeters
property real _landingAltitudeMeters: _missionItem.landingAltitude.rawValue
property real _loiterAltitudeMeters: _missionItem.loiterAltitude.rawValue
property real _landingAltitudeMeters: _missionItem.landingAltitude.rawValue
property real _finalApproachAltitudeMeters: _missionItem.finalApproachAltitude.rawValue
property bool _useLoiterToAlt: _missionItem.useLoiterToAlt.rawValue
property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate)
function _calcGlideSlopeHeights() {
var adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterTangentCoordinate)
var opposite = _loiterAltitudeMeters - _landingAltitudeMeters
var adjacent
if (_useLoiterToAlt) {
adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterTangentCoordinate)
} else {
adjacent = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate)
}
var opposite = _finalApproachAltitudeMeters - _landingAltitudeMeters
var angleRadians = Math.atan(opposite / adjacent)
var transitionDistance = _landingLengthMeters / 2
var glideSlopeDistance = adjacent - transitionDistance
......@@ -60,7 +66,7 @@ Item {
function showItemVisuals() {
if (objMgr.rgDynamicObjects.length === 0) {
_loiterPointObject = objMgr.createObject(loiterPointComponent, map, true /* parentObjectIsMap */)
_loiterPointObject = objMgr.createObject(finalApproachComponent, map, true /* parentObjectIsMap */)
_landingPointObject = objMgr.createObject(landingPointComponent, map, true /* parentObjectIsMap */)
var rgComponents = [ flightPathComponent, loiterRadiusComponent, landingAreaComponent, landingAreaLabelComponent,
......@@ -93,13 +99,17 @@ Item {
function showDragAreas() {
if (_dragAreas.length === 0) {
_dragAreas.push(loiterDragAreaComponent.createObject(map))
_dragAreas.push(finalApproachDragAreaComponent.createObject(map))
_dragAreas.push(landDragAreaComponent.createObject(map))
}
}
function _setFlightPath() {
_flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ]
if (_useLoiterToAlt) {
_flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ]
} else {
_flightPath = [ _missionItem.finalApproachCoordinate, _missionItem.landingCoordinate ]
}
}
QGCDynamicObjectManager {
......@@ -124,8 +134,9 @@ Item {
hideItemVisuals()
}
on_LandingAltitudeMetersChanged: _calcGlideSlopeHeights()
on_LoiterAltitudeMetersChanged: _calcGlideSlopeHeights()
on_LandingAltitudeMetersChanged: _calcGlideSlopeHeights()
on_FinalApproachAltitudeMetersChanged: _calcGlideSlopeHeights()
on_UseLoiterToAltChanged: { _calcGlideSlopeHeights(); _setFlightPath() }
Connections {
target: _missionItem
......@@ -170,6 +181,11 @@ Item {
_calcGlideSlopeHeights()
_setFlightPath()
}
onFinalApproachCoordinateChanged: {
_calcGlideSlopeHeights()
_setFlightPath()
}
}
// Mouse area to capture landing point coordindate
......@@ -194,14 +210,14 @@ Item {
}
}
// Control which is used to drag the loiter point
// Control which is used to drag the final approach point
Component {
id: loiterDragAreaComponent
id: finalApproachDragAreaComponent
MissionItemIndicatorDrag {
mapControl: _root.map
itemIndicator: _loiterPointObject
itemCoordinate: _missionItem.loiterCoordinate
itemCoordinate: _missionItem.finalApproachCoordinate
visible: _root.interactive
property bool _preventReentrancy: false
......@@ -211,8 +227,8 @@ Item {
if (Drag.active) {
_preventReentrancy = true
var angle = _missionItem.landingCoordinate.azimuthTo(itemCoordinate)
var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterCoordinate)
_missionItem.loiterCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle)
var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate)
_missionItem.finalApproachCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle)
_preventReentrancy = false
}
}
......@@ -246,20 +262,20 @@ Item {
}
}
// Loiter point
// Final approach point
Component {
id: loiterPointComponent
id: finalApproachComponent
MapQuickItem {
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.loiterCoordinate
coordinate: _missionItem.finalApproachCoordinate
sourceItem:
MissionItemIndexLabel {
index: _missionItem.sequenceNumber
label: qsTr("Loiter")
label: _useLoiterToAlt ? qsTr("Loiter") : qsTr("Approach")
checked: _missionItem.isCurrentItem
onClicked: _root.clicked(_missionItem.sequenceNumber)
......@@ -292,11 +308,12 @@ Item {
MapCircle {
z: QGroundControl.zOrderMapItems
center: _missionItem.loiterCoordinate
center: _missionItem.finalApproachCoordinate
radius: _missionItem.loiterRadius.rawValue
border.width: 2
border.color: "green"
color: "transparent"
visible: _useLoiterToAlt
}
}
......@@ -347,6 +364,7 @@ Item {
z: QGroundControl.zOrderMapItems
visible: _missionItem.isCurrentItem
sourceItem: QGCLabel {
id: glideSlopeLabel
text: qsTr("Glide Slope")
......@@ -383,6 +401,7 @@ Item {
target: _missionItem
onLandingCoordinateChanged: recalc()
onLoiterTangentCoordinateChanged: recalc()
onFinalApproachCoordinateChanged: recalc()
}
}
}
......@@ -415,6 +434,7 @@ Item {
target: _missionItem
onLandingCoordinateChanged: recalc()
onLoiterTangentCoordinateChanged: recalc()
onFinalApproachCoordinateChanged: recalc()
}
}
}
......@@ -437,7 +457,7 @@ Item {
path = [ ]
addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, _landingAreaBearing - angleDegrees))
addCoordinate(_missionItem.landingCoordinate.atDistanceAndAzimuth(hypotenuse, _landingAreaBearing + angleDegrees))
addCoordinate(_missionItem.loiterTangentCoordinate)
addCoordinate(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate)
}
Component.onCompleted: recalc()
......@@ -446,6 +466,12 @@ Item {
target: _missionItem
onLandingCoordinateChanged: recalc()
onLoiterTangentCoordinateChanged: recalc()
onFinalApproachCoordinateChanged: recalc()
}
Connections {
target: _missionItem.useLoiterToAlt
onRawValueChanged: recalc()
}
}
}
......@@ -477,8 +503,8 @@ Item {
target: _missionItem
onLandingCoordinateChanged: recalc()
onLoiterTangentCoordinateChanged: recalc()
onFinalApproachCoordinateChanged: recalc()
}
}
}
......@@ -499,7 +525,7 @@ Item {
function recalc() {
var transitionCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(_landingLengthMeters / 2, _landingAreaBearing)
var halfDistance = transitionCoordinate.distanceTo(_missionItem.loiterTangentCoordinate) / 2
var halfDistance = transitionCoordinate.distanceTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate) / 2
var centeredCoordinate = transitionCoordinate.atDistanceAndAzimuth(halfDistance, _landingAreaBearing)
var angleIncrement = _landingAreaBearing > 180 ? -90 : 90
coordinate = centeredCoordinate.atDistanceAndAzimuth(_landingWidthMeters / 2, _landingAreaBearing + angleIncrement)
......@@ -511,8 +537,13 @@ Item {
target: _missionItem
onLandingCoordinateChanged: recalc()
onLoiterTangentCoordinateChanged: recalc()
onFinalApproachCoordinateChanged: recalc()
}
Connections {
target: _missionItem.useLoiterToAlt
onRawValueChanged: recalc()
}
}
}
......@@ -524,11 +555,11 @@ Item {
anchorPoint.y: 0
z: QGroundControl.zOrderMapItems
visible: _missionItem.isCurrentItem
coordinate: _missionItem.loiterTangentCoordinate
coordinate: _useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate
sourceItem: HeightIndicator {
map: _root.map
heightText: _missionItem.loiterAltitude.value.toFixed(1) + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString
heightText: _missionItem.finalApproachAltitude.value.toFixed(1) + QGroundControl.unitsConversion.appSettingsHorizontalDistanceUnitsString
}
}
}
......
src/PlanView/VTOLLandingPatternEditor.qml
View file @ 48ffa39e
......@@ -51,29 +51,27 @@ Rectangle {
spacing: _margin
visible: !editorColumnNeedLandingPoint.visible
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
font.pointSize: ScreenTools.smallFontPointSize
text: qsTr("Loiter down to specified altitude. Fly to land point while transitioning. Hover straight down to land.")
}
SectionHeader {
id: loiterPointSection
id: finalApproachSection
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("Loiter point")
text: qsTr("Final approach")
}
Column {
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
visible: loiterPointSection.checked
visible: finalApproachSection.checked
Item { width: 1; height: _spacer }
FactCheckBox {
text: qsTr("Use loiter to altitude")
fact: missionItem.useLoiterToAlt
visible: missionItem.useLoiterToAlt.visible
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
......@@ -83,24 +81,28 @@ Rectangle {
AltitudeFactTextField {
Layout.fillWidth: true
fact: missionItem.loiterAltitude
fact: missionItem.finalApproachAltitude
altitudeMode: _altitudeMode
}
QGCLabel { text: qsTr("Radius") }
QGCLabel {
text: qsTr("Radius")
visible: missionItem.useLoiterToAlt.rawValue
}
FactTextField {
Layout.fillWidth: true
fact: missionItem.loiterRadius
visible: missionItem.useLoiterToAlt.rawValue
}
}
Item { width: 1; height: _spacer }
QGCCheckBox {
text: qsTr("Loiter clockwise")
checked: missionItem.loiterClockwise
onClicked: missionItem.loiterClockwise = checked
FactCheckBox {
text: qsTr("Loiter clockwise")
fact: missionItem.loiterClockwise
visible: missionItem.useLoiterToAlt.rawValue
}
QGCButton {
......@@ -222,7 +224,7 @@ Rectangle {
wrapMode: Text.WordWrap
color: qgcPal.warningText
font.pointSize: ScreenTools.smallFontPointSize
text: qsTr("* Avoid tailwind from loiter to land.")
text: qsTr("* Avoid tailwind on approach to land.")
}
QGCLabel {
......
src/PlanView/VTOLLandingPatternMapVisual.qml
View file @ 48ffa39e
......@@ -33,9 +33,10 @@ Item {
property var _mouseArea
property var _dragAreas: [ ]
property var _flightPath
property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_missionItem.loiterTangentCoordinate)
property var _loiterPointObject
property var _landingPointObject
property bool _useLoiterToAlt: _missionItem.useLoiterToAlt.rawValue
property real _landingAreaBearing: _missionItem.landingCoordinate.azimuthTo(_useLoiterToAlt ? _missionItem.loiterTangentCoordinate : _missionItem.finalApproachCoordinate)
function hideItemVisuals() {
objMgr.destroyObjects()
......@@ -43,7 +44,7 @@ Item {
function showItemVisuals() {
if (objMgr.rgDynamicObjects.length === 0) {
_loiterPointObject = objMgr.createObject(loiterPointComponent, map, true /* parentObjectIsMap */)
_loiterPointObject = objMgr.createObject(finalApproachPointComponent, map, true /* parentObjectIsMap */)
_landingPointObject = objMgr.createObject(landingPointComponent, map, true /* parentObjectIsMap */)
var rgComponents = [ flightPathComponent, loiterRadiusComponent ]
......@@ -80,7 +81,11 @@ Item {
}
function _setFlightPath() {
_flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ]
if (_useLoiterToAlt) {
_flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ]
} else {
_flightPath = [ _missionItem.finalApproachCoordinate, _missionItem.landingCoordinate ]
}
}
QGCDynamicObjectManager {
......@@ -105,6 +110,8 @@ Item {
hideItemVisuals()
}
on_UseLoiterToAltChanged: _setFlightPath()
Connections {
target: _missionItem
......@@ -141,6 +148,7 @@ Item {
onLandingCoordinateChanged: _setFlightPath()
onLoiterTangentCoordinateChanged: _setFlightPath()
onFinalApproachCoordinateChanged: _setFlightPath()
}
// Mouse area to capture landing point coordindate
......@@ -164,16 +172,28 @@ Item {
}
}
// Control which is used to drag the loiter point
// Control which is used to drag the final approach point
Component {
id: loiterDragAreaComponent
MissionItemIndicatorDrag {
mapControl: _root.map
itemIndicator: _loiterPointObject
itemCoordinate: _missionItem.loiterCoordinate
onItemCoordinateChanged: _missionItem.loiterCoordinate = itemCoordinate
itemCoordinate: _missionItem.finalApproachCoordinate
property bool _preventReentrancy: false
onItemCoordinateChanged: {
if (!_preventReentrancy) {
if (Drag.active) {
_preventReentrancy = true
var angle = _missionItem.landingCoordinate.azimuthTo(itemCoordinate)
var distance = _missionItem.landingCoordinate.distanceTo(_missionItem.finalApproachCoordinate)
_missionItem.finalApproachCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(distance, angle)
_preventReentrancy = false
}
}
}
}
}
......@@ -202,20 +222,20 @@ Item {
}
}
// Loiter point
// Final approach point
Component {
id: loiterPointComponent
id: finalApproachPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.loiterCoordinate
coordinate: _missionItem.finalApproachCoordinate
sourceItem:
MissionItemIndexLabel {
index: _missionItem.sequenceNumber
label: qsTr("Loiter")
label: _useLoiterToAlt ? qsTr("Loiter") : qsTr("Approach")
checked: _missionItem.isCurrentItem
onClicked: _root.clicked(_missionItem.sequenceNumber)
......@@ -249,11 +269,12 @@ Item {
MapCircle {
z: QGroundControl.zOrderMapItems
center: _missionItem.loiterCoordinate
center: _missionItem.finalApproachCoordinate
radius: _missionItem.loiterRadius.rawValue
border.width: 2
border.color: "green"
color: "transparent"
visible: _useLoiterToAlt
}
}
}
src/qgcunittest/MultiSignalSpy.cc
View file @ 48ffa39e
......@@ -59,7 +59,7 @@ bool MultiSignalSpy::init(QObject* signalEmitter, ///< [in] object whi
for (size_t i=0; i<_cSignals; i++) {
_rgSpys[i] = new QSignalSpy(_signalEmitter, _rgSignals[i]);
if (!_rgSpys[i]->isValid()) {
qDebug() << "Invalid signal";
qDebug() << "Invalid signal: index" << i;
return false;
}
}
......
src/qgcunittest/UnitTest.cc
View file @ 48ffa39e
......@@ -525,3 +525,8 @@ bool UnitTest::fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordi
{
return coord1.distanceTo(coord2) < 1.0;
}
QGeoCoordinate UnitTest::changeCoordinateValue(const QGeoCoordinate& coordinate)
{
return coordinate.atDistanceAndAzimuth(1, 0);
}
src/qgcunittest/UnitTest.h
View file @ 48ffa39e
......@@ -100,6 +100,8 @@ public:
/// @param increment 0 use standard increment, other increment by specified amount if double value
void changeFactValue(Fact* fact, double increment = 0);
QGeoCoordinate changeCoordinateValue(const QGeoCoordinate& coordinate);
/// Returns true is the position of the two coordinates is less then a meter from each other.
/// Does not check altitude.
static bool fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordinate& coord2);
......
src/qgcunittest/UnitTestList.cc
View file @ 48ffa39e
......@@ -49,6 +49,7 @@
#include "InitialConnectTest.h"
#include "FTPManagerTest.h"
#include "MissionCommandTreeEditorTest.h"
#include "LandingComplexItemTest.h"
UT_REGISTER_TEST(FactSystemTestGeneric)
UT_REGISTER_TEST(FactSystemTestPX4)
......@@ -84,6 +85,7 @@ UT_REGISTER_TEST(TransectStyleComplexItemTest)
UT_REGISTER_TEST(QGCMapPolylineTest)
UT_REGISTER_TEST(CameraCalcTest)
UT_REGISTER_TEST(FWLandingPatternTest)
UT_REGISTER_TEST(LandingComplexItemTest)
UT_REGISTER_TEST_STANDALONE(MissionCommandTreeEditorTest)
// List of unit test which are currently disabled.
......
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