Skip to content

Commit

Permalink
Support Fixed Wing Landing Pattern file persistence
Browse files Browse the repository at this point in the history
  • Loading branch information
@DonLakeFlyer
DonLakeFlyer committed Mar 5, 2017
1 parent 45cb473 commit 14474c9
Show file tree
Hide file tree
Showing 3 changed files with 102 additions and 24 deletions.
107 changes: 83 additions & 24 deletions src/MissionManager/FixedWingLandingComplexItem.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,13 @@ const char* FixedWingLandingComplexItem::_loiterAltitudeName = "Loiter
const char* FixedWingLandingComplexItem::_loiterRadiusName = "Loiter radius";
const char* FixedWingLandingComplexItem::_landingAltitudeName = "Landing altitude";

const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate";
const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius";
const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey = "loiterClockwise";
const char* FixedWingLandingComplexItem::_jsonLoiterAltitudeRelativeKey = "loiterAltitudeRelative";
const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate";
const char* FixedWingLandingComplexItem::_jsonLandingAltitudeRelativeKey = "landAltitudeRelative";

QMap<QString, FactMetaData*> FixedWingLandingComplexItem::_metaDataMap;

FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent)
Expand Down Expand Up @@ -60,6 +67,8 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
_landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue());
_landingAltitudeFact.setRawValue (_landingAltitudeFact.rawDefaultValue());

connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
connect(&_loiterToLandDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
Expand All @@ -86,7 +95,23 @@ void FixedWingLandingComplexItem::save(QJsonObject& saveObject) const
saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue;

// FIXME: Need real implementation
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[_jsonLoiterClockwiseKey] = _loiterClockwise;
saveObject[_jsonLoiterAltitudeRelativeKey] = _loiterAltitudeRelative;
saveObject[_jsonLandingAltitudeRelativeKey] = _landingAltitudeRelative;
}

void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber)
Expand All @@ -100,32 +125,57 @@ void FixedWingLandingComplexItem::setSequenceNumber(int sequenceNumber)

bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
{
// FIXME: Need real implementation
Q_UNUSED(complexObject);
Q_UNUSED(sequenceNumber);
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 },
{ _jsonLoiterAltitudeRelativeKey, QJsonValue::Bool, true },
{ _jsonLandingCoordinateKey, QJsonValue::Array, true },
{ _jsonLandingAltitudeRelativeKey, QJsonValue::Bool, true },
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
return false;
}

errorString = "NYI";
return false;
}
QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) {
errorString = tr("QGroundControl does not support loading this complex mission item type: %1:2").arg(itemType).arg(complexType);
return false;
}

double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
// FIXME: Need real implementation
Q_UNUSED(other);
setSequenceNumber(sequenceNumber);

double greatestDistance = 0.0;
QGeoCoordinate coordinate;
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
_loiterCoordinate = coordinate;
_loiterAltitudeFact.setRawValue(coordinate.altitude());

#if 0
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate currentCoord = _gridPoints[i].value<QGeoCoordinate>();
double distance = currentCoord.distanceTo(other);
if (distance > greatestDistance) {
greatestDistance = distance;
}
if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLandingCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
return false;
}
#endif
_landingCoordinate = coordinate;
_landingAltitudeFact.setRawValue(coordinate.altitude());

_loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble());
_loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool();
_loiterAltitudeRelative = complexObject[_jsonLoiterAltitudeRelativeKey].toBool();
_landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();

return greatestDistance;
_landingCoordSet = true;
_recalcFactsFromCoords();

return true;
}

double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
return qMax(_loiterCoordinate.distanceTo(other),_landingCoordinate.distanceTo(other));
}

bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
Expand Down Expand Up @@ -181,13 +231,12 @@ QmlObjectListModel* FixedWingLandingComplexItem::getMissionItems(void) const

double FixedWingLandingComplexItem::complexDistance(void) const
{
// FIXME: Need real implementation
return 0;
return _loiterCoordinate.distanceTo(_landingCoordinate);
}

void FixedWingLandingComplexItem::setCruiseSpeed(double cruiseSpeed)
{
// FIXME: Need real implementation
// We don't care about cruise speed
Q_UNUSED(cruiseSpeed);
}

Expand Down Expand Up @@ -296,3 +345,13 @@ void FixedWingLandingComplexItem::_recalcFactsFromCoords(void)
_ignoreRecalcSignals = false;
}
}

void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
}

void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{
_landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
}
9 changes: 9 additions & 0 deletions src/MissionManager/FixedWingLandingComplexItem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,8 @@ class FixedWingLandingComplexItem : public ComplexMissionItem
private slots:
void _recalcLoiterCoordFromFacts(void);
void _recalcFactsFromCoords(void);
void _updateLoiterCoodinateAltitudeFromFact(void);
void _updateLandingCoodinateAltitudeFromFact(void);

private:
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
Expand Down Expand Up @@ -121,6 +123,13 @@ private slots:
static const char* _loiterRadiusName;
static const char* _landingHeadingName;
static const char* _landingAltitudeName;

static const char* _jsonLoiterCoordinateKey;
static const char* _jsonLoiterRadiusKey;
static const char* _jsonLoiterClockwiseKey;
static const char* _jsonLoiterAltitudeRelativeKey;
static const char* _jsonLandingCoordinateKey;
static const char* _jsonLandingAltitudeRelativeKey;
};

#endif
10 changes: 10 additions & 0 deletions src/MissionManager/MissionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -488,6 +488,16 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje
qCDebug(MissionControllerLog) << "Survey load complete: nextSequenceNumber" << nextSequenceNumber;
visualItems->append(surveyItem);
complexItems->append(surveyItem);
} else if (complexItemType == FixedWingLandingComplexItem::jsonComplexItemTypeValue) {
qCDebug(MissionControllerLog) << "Loading Fixed Wing Landing Pattern: nextSequenceNumber" << nextSequenceNumber;
FixedWingLandingComplexItem* landingItem = new FixedWingLandingComplexItem(vehicle, visualItems);
if (!landingItem->load(itemObject, nextSequenceNumber++, errorString)) {
return false;
}
nextSequenceNumber = landingItem->lastSequenceNumber() + 1;
qCDebug(MissionControllerLog) << "FW Landing Pattern load complete: nextSequenceNumber" << nextSequenceNumber;
visualItems->append(landingItem);
complexItems->append(landingItem);
} else {
errorString = tr("Unsupported complex item type: %1").arg(complexItemType);
}
Expand Down

0 comments on commit 14474c9

Please sign in to comment.