Commit 99e5ae38 authored by Valentin Platzgummer's avatar Valentin Platzgummer

addapting wima stuff to qgc master branch, code not ready

parent 5e177398
...@@ -118,7 +118,7 @@ public: ...@@ -118,7 +118,7 @@ public:
QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); } QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final; void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final; void applyNewAltitude (double newAltitude) final;
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const final; ReadyForSaveState readyForSaveState (void) const final;
double additionalTimeDelay (void) const final; double additionalTimeDelay (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return true; } bool exitCoordinateSameAsEntry (void) const final { return true; }
......
...@@ -88,8 +88,8 @@ public: ...@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem // Overrides from VisualMissionItem
void save (QJsonArray& planItems) override = 0; void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0; bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final; virtual void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final; virtual void applyNewAltitude (double newAltitude) final;
bool dirty (void) const final { return _dirty; } bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; } bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; } bool isStandaloneCoordinate (void) const final { return false; }
...@@ -100,7 +100,7 @@ public: ...@@ -100,7 +100,7 @@ public:
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); } double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (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(); } double specifiedGimbalPitch (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const override; ReadyForSaveState readyForSaveState (void) const override;
QString commandDescription (void) const override { return tr("Transect"); } QString commandDescription (void) const override { return tr("Transect"); }
QString commandName (void) const override { return tr("Transect"); } QString commandName (void) const override { return tr("Transect"); }
......
...@@ -19,6 +19,8 @@ ...@@ -19,6 +19,8 @@
#include "CircularGenerator.h" #include "CircularGenerator.h"
#include "LinearGenerator.h" #include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog") QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template <typename T> template <typename T>
...@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey"; ...@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey"; const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::variantName = "Variant"; const char *CircularSurvey::variantName = "Variant";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, CircularSurvey::CircularSurvey(PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent) const QString &kmlOrShpFile, QObject *parent)
: TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent), : TransectStyleComplexItem(masterController, flyView, settingsGroup, parent),
_state(STATE::IDLE), _state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile( _metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)), QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
...@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, ...@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_setTransects); &CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this, connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
&CircularSurvey::calculatingChanged); &CircularSurvey::calculatingChanged);
this->_transectsDirty = true;
// Altitude
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::coordinateHasRelativeAltitudeChanged);
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::exitCoordinateHasRelativeAltitudeChanged);
// Register Generators. // Register Generators.
auto lg = std::make_shared<routing::LinearGenerator>(this->_pAreaData); auto lg = std::make_shared<routing::LinearGenerator>(this->_pAreaData);
...@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, ...@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return false; return false;
} }
if (!_load(complexObject, errorString)) { if (!load(complexObject, sequenceNumber, errorString)) {
_ignoreRecalc = false; _ignoreRecalc = false;
return false; return false;
} }
...@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, ...@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc = false; _ignoreRecalc = false;
_recalcComplexDistance();
if (_cameraShots == 0) { if (_cameraShots == 0) {
// Shot count was possibly not available from plan file // Shot count was possibly not available from plan file
_recalcCameraShots(); _recalcCameraShots();
...@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) { ...@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool CircularSurvey::specifiesCoordinate() const { return true; } bool CircularSurvey::specifiesCoordinate() const { return true; }
void CircularSurvey::appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
if (_loadedMissionItems.count()) {
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems(items, missionItemParent);
} else {
// Build the mission items on the fly
_buildAndAppendMissionItems(items, missionItemParent);
}
}
void CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty)
return;
int seqNum = _sequenceNumber;
for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
item->setSequenceNumber(seqNum++);
items.append(item);
}
}
void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) {
if (_transectsDirty || _transects.count() == 0)
return;
MissionItem *item;
int seqNum = _sequenceNumber;
MAV_FRAME mavFrame =
followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
? MAV_FRAME_GLOBAL
: MAV_FRAME_GLOBAL_RELATIVE_ALT;
for (const QList<TransectStyleComplexItem::CoordInfo_t> &transect :
_transects) {
// bool transectEntry = true;
for (const CoordInfo_t &transectCoordInfo : transect) {
item = new MissionItem(
seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
0, // Hold time (delay for hover and capture to settle vehicle
// before image is taken)
0.0, // No acceptance radius specified
0.0, // Pass through waypoint
std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
transectCoordInfo.coord.latitude(),
transectCoordInfo.coord.longitude(),
transectCoordInfo.coord.altitude(),
true, // autoContinue
false, // isCurrentItem
missionItemParent);
items.append(item);
}
}
}
bool CircularSurvey::_switchToGenerator( bool CircularSurvey::_switchToGenerator(
const CircularSurvey::PtrGenerator &newG) { const CircularSurvey::PtrGenerator &newG) {
if (this->_pGenerator != newG) { if (this->_pGenerator != newG) {
...@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() { ...@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
} }
void CircularSurvey::_updateWorker() { void CircularSurvey::_updateWorker() {
// Mark transects as dirty.
this->_transectsDirty = true;
// Reset data. // Reset data.
this->_transects.clear(); this->_transects.clear();
this->_variantVector.clear(); this->_variantVector.clear();
...@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() { ...@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
} }
} }
void CircularSurvey::applyNewAltitude(double newAltitude) {
_cameraCalc.valueSetIsDistance()->setRawValue(true);
_cameraCalc.distanceToSurface()->setRawValue(newAltitude);
_cameraCalc.setDistanceToSurfaceRelative(true);
}
double CircularSurvey::timeBetweenShots() { return 1; } double CircularSurvey::timeBetweenShots() { return 1; }
QString CircularSurvey::commandDescription() const { QString CircularSurvey::commandDescription() const {
...@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); } ...@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); } QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const { TransectStyleComplexItem::ReadyForSaveState
return TransectStyleComplexItem::readyForSave() && !_transectsDirty && CircularSurvey::readyForSaveState() const {
this->_state == STATE::IDLE; if (TransectStyleComplexItem::readyForSaveState() ==
TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) {
if (this->_state == STATE::IDLE) {
return ReadyForSaveState::ReadyForSave;
} else {
return ReadyForSaveState::NotReadyForSaveData;
}
} else {
return TransectStyleComplexItem::readyForSaveState();
}
} }
double CircularSurvey::additionalTimeDelay() const { return 0; } double CircularSurvey::additionalTimeDelay() const { return 0; }
...@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { ...@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<< " ms"; << " ms";
} }
void CircularSurvey::_recalcComplexDistance() {
_complexDistance = 0;
if (_transectsDirty)
return;
for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
_complexDistance +=
_visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(
_visualTransectPoints[i + 1].value<QGeoCoordinate>());
}
emit complexDistanceChanged();
}
// no cameraShots in Circular Survey, add if desired // no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; } void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
...@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) { ...@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&CircularSurvey::_changeVariant); &CircularSurvey::_changeVariant);
this->_changeVariantWorker(); this->_changeVariantWorker();
// Mark transect ready.
this->_transectsDirty = false;
this->_state = STATE::SKIPP; this->_state = STATE::SKIPP;
this->_rebuildTransects(); this->_rebuildTransects();
} else { } else {
......
...@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem { ...@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using PtrGenerator = std::shared_ptr<routing::GeneratorBase>; using PtrGenerator = std::shared_ptr<routing::GeneratorBase>;
using PtrRoutingData = std::shared_ptr<RoutingData>; using PtrRoutingData = std::shared_ptr<RoutingData>;
using PtrWorker = std::shared_ptr<RoutingThread>; using PtrWorker = std::unique_ptr<RoutingThread>;
using Transects = QList<QList<CoordInfo_t>>;
using Variant = Transects;
public: public:
/// @param vehicle Vehicle which this is being contructed for CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile,
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile,
QObject *parent); QObject *parent);
~CircularSurvey(); ~CircularSurvey();
...@@ -63,14 +60,11 @@ public: ...@@ -63,14 +60,11 @@ public:
QString mapVisualQML(void) const override final; QString mapVisualQML(void) const override final;
void save(QJsonArray &planItems) override final; void save(QJsonArray &planItems) override final;
bool specifiesCoordinate(void) const override final; bool specifiesCoordinate(void) const override final;
void appendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent) override final;
void applyNewAltitude(double newAltitude) override final;
double timeBetweenShots(void) override final; double timeBetweenShots(void) override final;
QString commandDescription(void) const override final; QString commandDescription(void) const override final;
QString commandName(void) const override final; QString commandName(void) const override final;
QString abbreviation(void) const override final; QString abbreviation(void) const override final;
bool readyForSave(void) const override final; ReadyForSaveState readyForSaveState(void) const override final;
double additionalTimeDelay(void) const override final; double additionalTimeDelay(void) const override final;
// Generator // Generator
...@@ -97,7 +91,6 @@ signals: ...@@ -97,7 +91,6 @@ signals:
private slots: private slots:
// Overrides from TransectStyleComplexItem // Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final; void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final; void _recalcCameraShots(void) final;
// Worker functions. // Worker functions.
...@@ -108,11 +101,6 @@ private slots: ...@@ -108,11 +101,6 @@ private slots:
void _reverseWorker(); void _reverseWorker();
private: private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
bool _switchToGenerator(const PtrGenerator &newG); bool _switchToGenerator(const PtrGenerator &newG);
// State. // State.
...@@ -139,7 +127,6 @@ private: ...@@ -139,7 +127,6 @@ private:
PtrGenerator _pGenerator; PtrGenerator _pGenerator;
// Routing. // Routing.
using Variant = Transects;
QVector<Variant> _variantVector; QVector<Variant> _variantVector;
PtrWorker _pWorker; PtrWorker _pWorker;
}; };
...@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, ...@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
} else if (retValue == JoinPolygonError::PathSizeLow) { } else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low."); qWarning("Polygon vertex count is low.");
} else { } else {
QVector<QGeoCoordinate> path; QList<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path); toGeoList(joinedPolygon, origin, path);
// qWarning("after transform"); // qWarning("after transform");
// qWarning() << path; // qWarning() << path;
...@@ -491,38 +491,6 @@ void WimaArea::init() { ...@@ -491,38 +491,6 @@ void WimaArea::init() {
&WimaArea::recalcInteractivity); &WimaArea::recalcInteractivity);
} }
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the console.
*/
void print(const WimaArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the \a outputString.
*/
void print(const WimaArea &area, QString &outputString) {
outputString.append(QString("Type: %1\n").arg(area.objectName()));
print(static_cast<const QGCMapPolygon &>(area), outputString);
outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude));
outputString.append(
QString("Border Polygon Offset: %1\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
outputString.append(
QString("Border Polygon Coordinates\n")
.arg(area._borderPolygonOffset.rawValue().toDouble()));
for (int i = 0; i < area._borderPolygon.count(); i++) {
QGeoCoordinate coordinate = area._borderPolygon.vertexCoordinate(i);
outputString.append(
QString("%1\n").arg(coordinate.toString(QGeoCoordinate::Degrees)));
}
}
// QDoc Documentation // QDoc Documentation
/*! /*!
......
...@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); } ...@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) { TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents(); this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) { for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles.get(i); const auto *obj = other.tiles[i];
const auto *tile = qobject_cast<const SnakeTile *>(obj); const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this)); this->tiles.append(new SnakeTile(*tile, this));
...@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) { ...@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
} }
bool TileData::operator==(const TileData &other) const { bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles && if (this->tileCenterPoints == other.tileCenterPoints &&
this->tileCenterPoints == other.tileCenterPoints; this->tiles.count() == other.tiles.count()) {
for (int i = 0; i < other.tiles.count(); ++i) {
if (this->tiles[i] != other.tiles[i]) {
return false;
}
}
return true;
} else {
return false;
}
} }
bool TileData::operator!=(const TileData &other) const { bool TileData::operator!=(const TileData &other) const {
...@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent) ...@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)), this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup, _minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName], _metaDataMap[minTileAreaName],
this /* QObject parent */)), this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)), this /* QObject parent */)),
_state(STATE::IDLE) { _state(STATE::IDLE) {
...@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, ...@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)), this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup, _minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName], _metaDataMap[minTileAreaName],
this /* QObject parent */)), this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)), this /* QObject parent */)),
_state(STATE::IDLE) { _state(STATE::IDLE) {
...@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; } ...@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) { void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) { if (ready()) {
this->WimaArea::saveToJson(json); this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble(); json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble(); json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble(); json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool(); json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName; json[areaTypeName] = WimaMeasurementAreaName;
} else { } else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing."; qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
} }
...@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() { ...@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) { if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter; const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter; const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height; const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter; const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea; const auto estNumTiles = totalArea / tileArea;
// Check some conditions. // Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES && if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) { this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING); setState(STATE::UPDATEING);
auto polygon = this->coordinateList(); auto polygon = this->coordinateList();
for (auto &v : polygon) { for (auto &v : polygon) {
v.setAltitude(0); v.setAltitude(0);
} }
const auto minArea = const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea; this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
auto *th = this->thread(); auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] { auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData()); DataPtr pData(new TileData());
// Convert to ENU system. // Convert to ENU system.
QGeoCoordinate origin = polygon.first(); QGeoCoordinate origin = polygon.first();
FPolygon polygonENU; FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU); areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU; std::vector<FPolygon> tilesENU;
BoundingBox bbox; BoundingBox bbox;
std::string errorString; std::string errorString;
// Generate tiles. // Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) { errorString)) {
// Convert to geo system. // Convert to geo system.
for (const auto &t : tilesENU) { for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get()); auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) { for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex; QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex); fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex); geoTile->push_back(geoVertex);
}
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
} }
pData->tiles.append(geoTile);
// Calculate center.
snake::FPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
} }
} pData->moveToThread(th);
pData->moveToThread(th);
qCDebug(WimaMeasurementAreaLog) qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: " << "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>( << std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start) std::chrono::high_resolution_clock::now() - start)
.count() .count()
<< " ms"; << " ms";
return pData; return pData;
}); // QtConcurrent::run() }); // QtConcurrent::run()
this->_watcher.setFuture(future); this->_watcher.setFuture(future);
} }
} }
qCDebug(WimaMeasurementAreaLog) qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: " << "doUpdate(): execution time: "
......
#include "WimaServiceArea.h" #include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog")
const char *WimaServiceArea::wimaServiceAreaName = "Service Area"; const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude"; const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude"; const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
...@@ -102,20 +106,24 @@ void WimaServiceArea::init() { ...@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
} else if (this->_depot.isValid()) { } else if (this->_depot.isValid()) {
// Use nearest coordinate. // Use nearest coordinate.
auto minDist = std::numeric_limits<double>::infinity(); auto minDist = std::numeric_limits<double>::infinity();
auto minIt = this->pathReference().begin(); int minIndex = 0;
for (auto it = this->pathReference().begin(); for (int idx = 0; idx < this->pathModel().count(); ++idx) {
it < this->pathReference().end(); ++it) { const QObject *obj = this->pathModel()[idx];
const auto vertex = it->value<QGeoCoordinate>(); const auto *vertex = qobject_cast<const QGeoCoordinate *>(obj);
auto d = vertex.distanceTo(this->_depot); if (vertex != nullptr) {
if (d < minDist) { auto d = vertex->distanceTo(this->_depot);
minDist = d; if (d < minDist) {
minIt = it; minDist = d;
minIndex = idx;
}
} else {
qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!";
} }
} }
this->setDepot(minIt->value<QGeoCoordinate>()); this->setDepot(*this->pathModel().value<QGeoCoordinate *>(minIndex));
} else if (this->pathReference().size() > 0) { } else if (this->pathModel().count() > 0) {
// Use first coordinate. // Use first coordinate.
this->setDepot(this->pathReference().value(0).value<QGeoCoordinate>()); this->setDepot(*this->pathModel().value<QGeoCoordinate *>(0));
} }
} }
}); });
......
...@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) { ...@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) { if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) { for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) { if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i); const auto *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
snake::FPolygon tileENU; snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU); snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
...@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) { ...@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) { if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) { for (int i = 0; i < tiles->count(); ++i) {
if (progress[i] == 100) { if (progress[i] == 100) {
const auto *tile = tiles->value<const SnakeTile *>(i); const QObject *obj = (*tiles)[int(i)];
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
snake::FPolygon tileENU; snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU); snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
...@@ -97,7 +97,7 @@ private: ...@@ -97,7 +97,7 @@ private:
}; };
using StatusMap = std::map<NemoInterface::STATUS, QString>; using StatusMap = std::map<NemoInterface::STATUS, QString>;
StatusMap statusMap{ static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>( std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>( std::make_pair<NemoInterface::STATUS, QString>(
...@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p) ...@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionString = connectionStringFact->rawValue().toString(); auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString( if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) { connectionString.toLocal8Bit().data())) {
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else { } else {
QString defaultString("localhost:9090"); qgcApp()->warningMessageBoxOnMainThread(
qgcApp()->showMessage("ROS Bridge connection string invalid: " + "Nemo Interface",
connectionString); "Websocket connection string possibly invalid: " + connectionString +
qgcApp()->showMessage("Resetting connection string to: " + defaultString); ". Trying to connect anyways.");
connectionStringFact->setRawValue(
QVariant(defaultString)); // calls this function recursively
} }
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
}; };
connect(connectionStringFact, &SettingsFact::rawValueChanged, connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString); setConnectionString);
...@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { ...@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock); UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock); UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles.get(0); const auto *obj = tileData.tiles[0];
const auto *tile = qobject_cast<const SnakeTile *>(obj); const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
if (tile->coordinateList().size() > 0) { if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) { if (tile->coordinateList().first().isValid()) {
this->ENUOrigin = tile->coordinateList().first(); this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin; const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear(); this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) { for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles.get(i); obj = tileData.tiles[i];
tile = qobject_cast<const SnakeTile *>(obj); tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) { if (tile != nullptr) {
SnakeTileLocal tileENU; SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path()); snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU)); this->tilesENU.polygons().push_back(std::move(tileENU));
} else { } else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
break; break;
}
} }
} } else {
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid."; qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
} }
} else { } else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty."; qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
} }
}
} else { } else {
this->tileData.clear(); this->tileData.clear();
...@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() { ...@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg.progress().size() != progressMsg.progress().size() !=
requiredSize) { // Some error occured. requiredSize) { // Some error occured.
progressMsg.progress().clear(); progressMsg.progress().clear();
qgcApp()->showMessage("Invalid progress message received."); qgcApp()->informationMessageBoxOnMainThread(
"Nemo Interface", "Invalid progress message received.");
} }
emit this->parent->progressChanged(); emit this->parent->progressChanged();
...@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { ...@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (geographic_msgs::geo_point::toJson(origin, jOrigin, if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) { pDoc->GetAllocator())) {
lk.unlock(); lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else { } else {
lk.unlock(); lk.unlock();
qCWarning(NemoInterfaceLog) qCWarning(NemoInterfaceLog)
...@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { ...@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (jsk_recognition_msgs::polygon_array::toJson( if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) { this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock(); lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else { } else {
lk.unlock(); lk.unlock();
qCWarning(NemoInterfaceLog) qCWarning(NemoInterfaceLog)
...@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() { ...@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() && } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) { !this->topicServiceSetupDone) {
this->doTopicServiceSetup(); this->doTopicServiceSetup();
this->topicServiceSetupDone = true; this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED); this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() && } else if (this->pRosBridge->isRunning() &&
...@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() { ...@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType)); std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (jsk_recognition_msgs::polygon_array::toJson( if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) { this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
} else { } else {
qCWarning(NemoInterfaceLog) qCWarning(NemoInterfaceLog)
<< "Impl::publishTilesENU: could not create json document."; << "Impl::publishTilesENU: could not create json document.";
...@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() { ...@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType)); std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin, if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
jOrigin->GetAllocator())) { jOrigin->GetAllocator())) {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
} else { } else {
qCWarning(NemoInterfaceLog) qCWarning(NemoInterfaceLog)
<< "Impl::publishENUOrigin: could not create json document."; << "Impl::publishENUOrigin: could not create json document.";
......
...@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem( ...@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
using namespace WaypointManager::Utils; using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list, if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(), _settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) { &list /*parent*/, doUpdate /*do update*/)) {
qWarning( qWarning(
"WaypointManager::DefaultManager::next(): insertMissionItem failed."); "WaypointManager::DefaultManager::next(): insertMissionItem failed.");
...@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() { ...@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
} }
_currentMissionItems.clearAndDeleteContents(); _currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(), initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView()); _settings->isFlyView());
// Calculate path from home to first waypoint. // Calculate path from home to first waypoint.
...@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() { ...@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
Q_ASSERT(takeoffItem != nullptr); Q_ASSERT(takeoffItem != nullptr);
return false; return false;
} }
makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle()); makeTakeOffCmd(takeoffItem, _settings->masterController());
// Create change speed item. // Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
...@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() { ...@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
makeLandCmd(landItem, _settings->masterController()->managerVehicle()); makeLandCmd(landItem, _settings->masterController());
// Set altitude. // Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) { for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
...@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c, ...@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
using namespace WaypointManager::Utils; using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list, if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(), _settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) { &list /*parent*/, doUpdate /*do update*/)) {
qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed."); qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed.");
Q_ASSERT(false); Q_ASSERT(false);
...@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() { ...@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
} }
_currentMissionItems.clearAndDeleteContents(); _currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(), initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView()); _settings->isFlyView());
// Calculate path from vehicle to home position. // Calculate path from vehicle to home position.
...@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() { ...@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
qWarning("WaypointManager::RTLManager::next(): nullptr."); qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false; return false;
} }
makeLandCmd(landItem, _settings->masterController()->managerVehicle()); makeLandCmd(landItem, _settings->masterController());
// Set altitude. // Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) { for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
#include "Utils.h" #include "Utils.h"
#include "MissionSettingsItem.h" #include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream> #include <iostream>
#include <QGeoCoordinate> #include <QGeoCoordinate>
template<>
QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item)
{
return QVariant::fromValue(item->coordinate());
}
bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
long index, long index,
QmlObjectListModel &list, QmlObjectListModel &list,
Vehicle* vehicle, PlanMasterController* masterController,
bool flyView, bool flyView,
QObject *parent, QObject *parent,
bool doUpdates) bool doUpdates)
...@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, ...@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
} else { } else {
if (parent == nullptr) if (parent == nullptr)
parent = &list; parent = &list;
SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent); auto *newItem = new SimpleMissionItem(masterController, flyView, MissionItem(), parent);
int sequenceNumber = detail::nextSequenceNumber(list); int sequenceNumber = detail::nextSequenceNumber(list);
newItem->setSequenceNumber(sequenceNumber); newItem->setSequenceNumber(sequenceNumber);
...@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list) ...@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
} }
void WaypointManager::Utils::initialize(QmlObjectListModel &list, void WaypointManager::Utils::initialize(QmlObjectListModel &list,
Vehicle *vehicle, PlanMasterController *masterController,
bool flyView) bool flyView)
{ {
MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list); MissionSettingsItem* settingsItem = new MissionSettingsItem(masterController, flyView, &list);
list.insert(0, settingsItem); list.insert(0, settingsItem);
} }
...@@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list) ...@@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list)
} }
} }
bool WaypointManager::Utils::makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle)
{
if ( vehicle->fixedWing()
|| vehicle->vtol()
|| vehicle->multiRotor()
)
{
MAV_CMD takeoffCmd = vehicle->vtol()
? MAV_CMD_NAV_VTOL_TAKEOFF
: MAV_CMD_NAV_TAKEOFF;
if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) {
auto c = item->coordinate();
item->setCommand(takeoffCmd);
item->setCoordinate(c);
return true;
}
}
return false;
}
void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
{ {
assert(speed > 0); assert(speed > 0);
...@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) ...@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
item->setCoordinate(c); item->setCoordinate(c);
item->missionItem().setParam2(speed); item->missionItem().setParam2(speed);
} }
bool WaypointManager::Utils::makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle)
{
MAV_CMD landCmd = vehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
item->setCommand(landCmd);
} else {
Q_ASSERT(false);
qWarning("WaypointManager::Utils::makeLandCmd(): Land command not supported!");
return false;
}
return true;
}
This diff is collapsed.
...@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T value) { ...@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value); return static_cast<typename std::underlying_type<T>::type>(value);
} }
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms #define EVENT_TIMER_INTERVAL 50 // ms
const char *WimaController::areaItemsName = "AreaItems"; const char *WimaController::areaItemsName = "AreaItems";
...@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent) ...@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
_metaDataMap(FactMetaData::createMapFromJsonFile( _metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)), QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
_enableWimaController(settingsGroup, _enableWimaController(settingsGroup,
_metaDataMap[enableWimaControllerName]), _metaDataMap[enableWimaControllerName]){
_flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
_altitude(settingsGroup, _metaDataMap[altitudeName]),
_lowBatteryHandlingTriggered(false),
_batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
// Set up facts for waypoint manager.
connect(&_flightSpeed, &Fact::rawValueChanged, this,
&WimaController::_updateflightSpeed);
connect(&_altitude, &Fact::rawValueChanged, this,
&WimaController::_updateAltitude);
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this,
&WimaController::_eventTimerHandler);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// PlanData and Progress. // PlanData and Progress.
connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this, connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this,
&WimaController::planDataChangedHandler); &WimaController::planDataChangedHandler);
...@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) { ...@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
emit missionControllerChanged(); emit missionControllerChanged();
} }
void WimaController::requestSmartRTL() {
qCWarning(WimaControllerLog) << "requestSmartRTL() called";
QString errorString("Smart RTL requested.");
if (!_SRTLPrecondition(errorString)) {
qgcApp()->showMessage(errorString);
return;
}
emit smartRTLRequestConfirm();
}
bool WimaController::upload() { bool WimaController::upload() {
auto &items = _currentWM->currentMissionItems(); auto &items = _currentWM->currentMissionItems();
if (_masterController && _masterController->managerVehicle() && if (_masterController && _masterController->managerVehicle() &&
...@@ -141,68 +113,6 @@ bool WimaController::upload() { ...@@ -141,68 +113,6 @@ bool WimaController::upload() {
} }
} }
bool WimaController::forceUpload() {
auto &currentMissionItems = _currentWM->currentMissionItems();
if (currentMissionItems.count() < 1 || !_missionController ||
!_masterController) {
qCWarning(WimaControllerLog)
<< "forceUpload(): error:"
<< "currentMissionItems.count(): " << currentMissionItems.count()
<< "_missionController: " << _missionController
<< "_masterController: " << _masterController;
return false;
} else {
_missionController->removeAll();
// Set homeposition of settingsItem.
QmlObjectListModel *visuals = _missionController->visualItems();
MissionSettingsItem *settingsItem =
visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qCWarning(WimaControllerLog) << "updateCurrentMissionItems(): nullptr";
return false;
} else {
settingsItem->setCoordinate(_WMSettings.homePosition());
// Copy mission items to _missionController and send them.
for (int i = 1; i < currentMissionItems.count(); i++) {
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
}
_masterController->sendToVehicle();
return true;
}
}
}
void WimaController::removeFromVehicle() {
if (_masterController && _missionController) {
_masterController->removeAllFromVehicle();
_missionController->removeAll();
}
}
void WimaController::executeSmartRTL() {
qCWarning(WimaControllerLog) << "executeSmartRTL() called";
if (_masterController && _masterController->managerVehicle()) {
forceUpload();
_masterController->managerVehicle()->startMission();
}
}
void WimaController::initSmartRTL() {
qCWarning(WimaControllerLog) << "initSmartRTL() called";
_initSmartRTL();
}
void WimaController::removeVehicleTrajectoryHistory() {
if (_masterController && _masterController->managerVehicle()) {
_masterController->managerVehicle()
->trajectoryPoints()
->clearAndDeleteContents();
}
}
bool WimaController::_calcShortestPath(const QGeoCoordinate &start, bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination, const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) { QVector<QGeoCoordinate> &path) {
...@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() { ...@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
emit missionItemsChanged(); emit missionItemsChanged();
emit waypointPathChanged(); emit waypointPathChanged();
} }
void WimaController::_checkBatteryLevel() {
if (_missionController && _masterController &&
_masterController->managerVehicle()) {
Vehicle *managerVehicle = masterController()->managerVehicle();
WimaSettings *wimaSettings =
qgcApp()->toolbox()->settingsManager()->wimaSettings();
int threshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime =
wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
if (enabled) {
Fact *battery1percentRemaining =
managerVehicle->battery1FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining =
managerVehicle->battery2FactGroup()->getFact(
VehicleBatteryFactGroup::_percentRemainingFactName);
if (battery1percentRemaining->rawValue().toDouble() < threshold &&
battery2percentRemaining->rawValue().toDouble() < threshold) {
if (!_lowBatteryHandlingTriggered) {
_lowBatteryHandlingTriggered = true;
if (!(_missionController->remainingTime() <= minTime)) {
requestSmartRTL();
}
}
} else {
_lowBatteryHandlingTriggered = false;
}
}
}
}
void WimaController::_eventTimerHandler() {
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()
->toolbox()
->settingsManager()
->wimaSettings()
->enableLowBatteryHandling();
if (enableLowBatteryHandling->rawValue().toBool() &&
_batteryLevelTicker.ready())
_checkBatteryLevel();
}
void WimaController::_smartRTLCleanUp(bool flying) {
if (!flying && _missionController) { // vehicle has landed
_switchWaypointManager(_emptyWM);
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged,
this, &WimaController::_smartRTLCleanUp);
}
}
bool WimaController::_SRTLPrecondition(QString &errorString) {
if (!_planDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a "
"measurement and a service area."));
return false;
}
return _rtlWM.checkPrecondition(errorString);
}
void WimaController::_switchWaypointManager(
WaypointManager::ManagerBase &manager) {
if (_currentWM != &manager) {
_currentWM = &manager;
emit missionItemsChanged();
emit waypointPathChanged();
qCWarning(WimaControllerLog) << "_switchWaypointManager: statistics update "
"missing.";
}
}
void WimaController::_initSmartRTL() {
static int attemptCounter = 0;
attemptCounter++;
QString errorString;
if (_SRTLPrecondition(errorString)) {
if (_missionController && _masterController &&
_masterController->managerVehicle()) {
_masterController->managerVehicle()->pauseVehicle();
connect(_masterController->managerVehicle(), &Vehicle::flyingChanged,
this, &WimaController::_smartRTLCleanUp);
if (_rtlWM.update()) { // Calculate return path.
_switchWaypointManager(_rtlWM);
removeFromVehicle();
attemptCounter = 0;
emit smartRTLPathConfirm();
}
}
} else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
errorString.append(
tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this,
&WimaController::_initSmartRTL);
}
}
...@@ -70,10 +70,6 @@ public: ...@@ -70,10 +70,6 @@ public:
// Member Methodes // Member Methodes
Q_INVOKABLE WimaController *thisPointer(); Q_INVOKABLE WimaController *thisPointer();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other. // Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory(); Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload(); Q_INVOKABLE bool upload();
...@@ -97,9 +93,6 @@ signals: ...@@ -97,9 +93,6 @@ signals:
// Waypoints. // Waypoints.
void missionItemsChanged(void); void missionItemsChanged(void);
void waypointPathChanged(void); void waypointPathChanged(void);
// Smart RTL.
void smartRTLRequestConfirm(void);
void smartRTLPathConfirm(void);
// Upload. // Upload.
void forceUploadConfirm(void); void forceUploadConfirm(void);
...@@ -111,11 +104,6 @@ private slots: ...@@ -111,11 +104,6 @@ private slots:
QVector<QGeoCoordinate> &path); QVector<QGeoCoordinate> &path);
void _updateflightSpeed(void); void _updateflightSpeed(void);
void _updateAltitude(void); void _updateAltitude(void);
// SMART RTL
void _checkBatteryLevel(void);
bool _SRTLPrecondition(QString &errorString);
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake. // Snake.
void _switchWaypointManager(WaypointManager::ManagerBase &manager); void _switchWaypointManager(WaypointManager::ManagerBase &manager);
// Periodic tasks. // Periodic tasks.
...@@ -152,10 +140,6 @@ private: ...@@ -152,10 +140,6 @@ private:
SettingsFact _flightSpeed; // mission flight speed SettingsFact _flightSpeed; // mission flight speed
SettingsFact _altitude; // mission altitude SettingsFact _altitude; // mission altitude
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Periodic tasks. // Periodic tasks.
QTimer _eventTimer; QTimer _eventTimer;
EventTicker _batteryLevelTicker; EventTicker _batteryLevelTicker;
......
This diff is collapsed.
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