Skip to content

Q
qgroundcontrol
Commit 99e5ae38 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

addapting wima stuff to qgc master branch, code not ready

parent 5e177398
Hide whitespace changes
Inline Side-by-side
Showing with 320 additions and 702 deletions
src/MissionManager/SimpleMissionItem.h
View file @ 99e5ae38
......@@ -118,7 +118,7 @@ public:
QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) 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;
double additionalTimeDelay (void) const final;
bool exitCoordinateSameAsEntry (void) const final { return true; }
......
src/MissionManager/TransectStyleComplexItem.h
View file @ 99e5ae38
......@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
void save (QJsonArray& planItems) override = 0;
bool specifiesCoordinate (void) const override = 0;
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void applyNewAltitude (double newAltitude) final;
virtual void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
virtual void applyNewAltitude (double newAltitude) final;
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
......@@ -100,7 +100,7 @@ public:
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 setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
ReadyForSaveState readyForSaveState (void) const override;
QString commandDescription (void) const override { return tr("Transect"); }
QString commandName (void) const override { return tr("Transect"); }
......
src/Wima/CircularSurvey.cc
View file @ 99e5ae38
......@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template <typename T>
......@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::variantName = "Variant";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
CircularSurvey::CircularSurvey(PlanMasterController *masterController, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
: TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
: TransectStyleComplexItem(masterController, flyView, settingsGroup, parent),
_state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
......@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
&CircularSurvey::calculatingChanged);
this->_transectsDirty = true;
// Altitude
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::coordinateHasRelativeAltitudeChanged);
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::exitCoordinateHasRelativeAltitudeChanged);
// Register Generators.
auto lg = std::make_shared<routing::LinearGenerator>(this->_pAreaData);
......@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return false;
}
if (!_load(complexObject, errorString)) {
if (!load(complexObject, sequenceNumber, errorString)) {
_ignoreRecalc = false;
return false;
}
......@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc = false;
_recalcComplexDistance();
if (_cameraShots == 0) {
// Shot count was possibly not available from plan file
_recalcCameraShots();
......@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
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(
const CircularSurvey::PtrGenerator &newG) {
if (this->_pGenerator != newG) {
......@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
void CircularSurvey::_updateWorker() {
// Mark transects as dirty.
this->_transectsDirty = true;
// Reset data.
this->_transects.clear();
this->_variantVector.clear();
......@@ -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; }
QString CircularSurvey::commandDescription() const {
......@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString CircularSurvey::abbreviation() const { return tr("C.S."); }
bool CircularSurvey::readyForSave() const {
return TransectStyleComplexItem::readyForSave() && !_transectsDirty &&
this->_state == STATE::IDLE;
TransectStyleComplexItem::ReadyForSaveState
CircularSurvey::readyForSaveState() const {
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; }
......@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<< " 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
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
......@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&CircularSurvey::_changeVariant);
this->_changeVariantWorker();
// Mark transect ready.
this->_transectsDirty = false;
this->_state = STATE::SKIPP;
this->_rebuildTransects();
} else {
......
src/Wima/CircularSurvey.h
View file @ 99e5ae38
......@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using PtrGenerator = std::shared_ptr<routing::GeneratorBase>;
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:
/// @param vehicle Vehicle which this is being contructed for
/// @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,
CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile,
QObject *parent);
~CircularSurvey();
......@@ -63,14 +60,11 @@ public:
QString mapVisualQML(void) const override final;
void save(QJsonArray &planItems) 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;
QString commandDescription(void) const override final;
QString commandName(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;
// Generator
......@@ -97,7 +91,6 @@ signals:
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
// Worker functions.
......@@ -108,11 +101,6 @@ private slots:
void _reverseWorker();
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList<MissionItem *> &items,
QObject *missionItemParent);
bool _switchToGenerator(const PtrGenerator &newG);
// State.
......@@ -139,7 +127,6 @@ private:
PtrGenerator _pGenerator;
// Routing.
using Variant = Transects;
QVector<Variant> _variantVector;
PtrWorker _pWorker;
};
src/Wima/Geometry/WimaArea.cc
View file @ 99e5ae38
......@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
} else if (retValue == JoinPolygonError::PathSizeLow) {
qWarning("Polygon vertex count is low.");
} else {
QVector<QGeoCoordinate> path;
QList<QGeoCoordinate> path;
toGeoList(joinedPolygon, origin, path);
// qWarning("after transform");
// qWarning() << path;
......@@ -491,38 +491,6 @@ void WimaArea::init() {
&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
/*!
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 99e5ae38
......@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents();
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);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
......@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles &&
this->tileCenterPoints == other.tileCenterPoints;
if (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 {
......@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
this /* QObject parent */)),
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
......@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) {
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
this->WimaArea::saveToJson(json);
json[tileHeightName] = _tileHeight.rawValue().toDouble();
json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
json[showTilesName] = _showTiles.rawValue().toBool();
json[areaTypeName] = WimaMeasurementAreaName;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
......@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto start = std::chrono::high_resolution_clock::now();
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
auto start = std::chrono::high_resolution_clock::now();
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
FPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<FPolygon> tilesENU;
BoundingBox bbox;
std::string errorString;
// Generate tiles.
if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
errorString)) {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, 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)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
return pData;
}); // QtConcurrent::run()
return pData;
}); // QtConcurrent::run()
this->_watcher.setFuture(future);
}
this->_watcher.setFuture(future);
}
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
......
src/Wima/Geometry/WimaServiceArea.cc
View file @ 99e5ae38
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog")
const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
......@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
} else if (this->_depot.isValid()) {
// Use nearest coordinate.
auto minDist = std::numeric_limits<double>::infinity();
auto minIt = this->pathReference().begin();
for (auto it = this->pathReference().begin();
it < this->pathReference().end(); ++it) {
const auto vertex = it->value<QGeoCoordinate>();
auto d = vertex.distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIt = it;
int minIndex = 0;
for (int idx = 0; idx < this->pathModel().count(); ++idx) {
const QObject *obj = this->pathModel()[idx];
const auto *vertex = qobject_cast<const QGeoCoordinate *>(obj);
if (vertex != nullptr) {
auto d = vertex->distanceTo(this->_depot);
if (d < minDist) {
minDist = d;
minIndex = idx;
}
} else {
qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!";
}
}
this->setDepot(minIt->value<QGeoCoordinate>());
} else if (this->pathReference().size() > 0) {
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(minIndex));
} else if (this->pathModel().count() > 0) {
// Use first coordinate.
this->setDepot(this->pathReference().value(0).value<QGeoCoordinate>());
this->setDepot(*this->pathModel().value<QGeoCoordinate *>(0));
}
}
});
......
src/Wima/Snake/CircularGenerator.cpp
View file @ 99e5ae38
......@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
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) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/LinearGenerator.cpp
View file @ 99e5ae38
......@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if (progress.size() == tiles->count()) {
for (int i = 0; i < tiles->count(); ++i) {
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) {
snake::FPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
......
src/Wima/Snake/NemoInterface.cpp
View file @ 99e5ae38
......@@ -97,7 +97,7 @@ private:
};
using StatusMap = std::map<NemoInterface::STATUS, QString>;
StatusMap statusMap{
static StatusMap statusMap{
std::make_pair<NemoInterface::STATUS, QString>(
NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
std::make_pair<NemoInterface::STATUS, QString>(
......@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto connectionString = connectionStringFact->rawValue().toString();
if (ros_bridge::isValidConnectionString(
connectionString.toLocal8Bit().data())) {
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
QString defaultString("localhost:9090");
qgcApp()->showMessage("ROS Bridge connection string invalid: " +
connectionString);
qgcApp()->showMessage("Resetting connection string to: " + defaultString);
connectionStringFact->setRawValue(
QVariant(defaultString)); // calls this function recursively
qgcApp()->warningMessageBoxOnMainThread(
"Nemo Interface",
"Websocket connection string possibly invalid: " + connectionString +
". Trying to connect anyways.");
}
this->pRosBridge.reset(
new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
......@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock lk1(this->ENUOriginMutex, 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);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles.get(i);
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles[i];
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
break;
break;
}
}
}
} else {
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
}
} else {
qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
}
}
} else {
this->tileData.clear();
......@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg.progress().size() !=
requiredSize) { // Some error occured.
progressMsg.progress().clear();
qgcApp()->showMessage("Invalid progress message received.");
qgcApp()->informationMessageBoxOnMainThread(
"Nemo Interface", "Invalid progress message received.");
}
emit this->parent->progressChanged();
......@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
......@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
this->topicServiceSetupDone = true;
this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() &&
......@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishTilesENU: could not create json document.";
......@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
jOrigin->GetAllocator())) {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
} else {
qCWarning(NemoInterfaceLog)
<< "Impl::publishENUOrigin: could not create json document.";
......
src/Wima/WaypointManager/DefaultManager.cpp
View file @ 99e5ae38
......@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(),
_settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning(
"WaypointManager::DefaultManager::next(): insertMissionItem failed.");
......@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(),
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from home to first waypoint.
......@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
Q_ASSERT(takeoffItem != nullptr);
return false;
}
makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());
makeTakeOffCmd(takeoffItem, _settings->masterController());
// Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
......@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
makeLandCmd(landItem, _settings->masterController());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
src/Wima/WaypointManager/RTLManager.cpp
View file @ 99e5ae38
......@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(),
_settings->masterController(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
......@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(),
initialize(_currentMissionItems, _settings->masterController(),
_settings->isFlyView());
// Calculate path from vehicle to home position.
......@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
makeLandCmd(landItem, _settings->masterController());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
......
src/Wima/WaypointManager/Utils.cpp
View file @ 99e5ae38
#include "Utils.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream>
#include <QGeoCoordinate>
template<>
QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item)
{
return QVariant::fromValue(item->coordinate());
}
bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
long index,
QmlObjectListModel &list,
Vehicle* vehicle,
PlanMasterController* masterController,
bool flyView,
QObject *parent,
bool doUpdates)
......@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
} else {
if (parent == nullptr)
parent = &list;
SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent);
auto *newItem = new SimpleMissionItem(masterController, flyView, MissionItem(), parent);
int sequenceNumber = detail::nextSequenceNumber(list);
newItem->setSequenceNumber(sequenceNumber);
......@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
}
void WaypointManager::Utils::initialize(QmlObjectListModel &list,
Vehicle *vehicle,
PlanMasterController *masterController,
bool flyView)
{
MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list);
MissionSettingsItem* settingsItem = new MissionSettingsItem(masterController, flyView, &list);
list.insert(0, settingsItem);
}
......@@ -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)
{
assert(speed > 0);
......@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
item->setCoordinate(c);
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;
}
src/Wima/WaypointManager/Utils.h
View file @ 99e5ae38
......@@ -5,126 +5,106 @@
#include <QVariant>
class Vehicle;
class PlanMasterController;
class QGeoCoordinate;
namespace WaypointManager {
namespace Utils {
template<class CoordinateType>
CoordinateType getCoordinate(const SimpleMissionItem* item){
return item->coordinate();
template <class CoordinateType>
CoordinateType getCoordinate(const SimpleMissionItem *item) {
return item->coordinate();
}
template<>
QVariant getCoordinate(const SimpleMissionItem* item);
template <> QVariant getCoordinate(const SimpleMissionItem *item) {
return QVariant::fromValue(item->coordinate());
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList,
std::size_t startIndex,
std::size_t endIndex)
{
if ( startIndex < std::size_t(missionItems.count())
&& endIndex < std::size_t(missionItems.count())
&& missionItems.count() > 0) {
if (startIndex > endIndex) {
if ( !extract(missionItems,
coordinateList,
startIndex,
std::size_t(missionItems.count()-1)) /*recursion*/)
return false;
if ( !extract(missionItems,
coordinateList,
0,
endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
SimpleMissionItem *mItem = missionItems.value<SimpleMissionItem *>(int(i));
if (mItem == nullptr) {
coordinateList.clear();
return false;
}
coordinateList.append(getCoordinate<CoordinateType>(mItem));
}
}
} else
std::size_t startIndex, std::size_t endIndex) {
if (startIndex < std::size_t(missionItems.count()) &&
endIndex < std::size_t(missionItems.count()) &&
missionItems.count() > 0) {
if (startIndex > endIndex) {
if (!extract(missionItems, coordinateList, startIndex,
std::size_t(missionItems.count() - 1)) /*recursion*/)
return false;
if (!extract(missionItems, coordinateList, 0, endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
const QObject *obj = missionItems[int(i)];
auto *mItem = qobject_cast<const SimpleMissionItem *>(obj);
if (mItem == nullptr) {
coordinateList.clear();
return false;
}
coordinateList.append(getCoordinate<CoordinateType>(mItem));
}
}
} else
return false;
return true;
return true;
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const QmlObjectListModel &missionItems,
ContainerType<CoordinateType> &coordinateList)
{
return extract(missionItems,
coordinateList,
std::size_t(0),
std::size_t(missionItems.count())
);
}
ContainerType<CoordinateType> &coordinateList) {
return extract(missionItems, coordinateList, std::size_t(0),
std::size_t(missionItems.count()));
}
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList.
/// extracts the coordinates stored in missionItems (list of MissionItems) and
/// stores them in coordinateList.
template <class CoordinateType, template <class, class...> class ContainerType>
bool extract(const ContainerType<CoordinateType> &source,
ContainerType<CoordinateType> &destination,
std::size_t startIndex,
std::size_t endIndex)
{
if ( startIndex < std::size_t(source.size())
&& endIndex < std::size_t(source.size())
&& source.size() > 0) {
if (startIndex > endIndex) {
if ( !extract(source,
destination,
startIndex,
std::size_t(long(source.size())-1)) /*recursion*/)
return false;
if ( !extract(source,
destination,
0,
endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
destination.append(source[i]);
}
}
} else
bool extract(const ContainerType<CoordinateType> &source,
ContainerType<CoordinateType> &destination, std::size_t startIndex,
std::size_t endIndex) {
if (startIndex < std::size_t(source.size()) &&
endIndex < std::size_t(source.size()) && source.size() > 0) {
if (startIndex > endIndex) {
if (!extract(source, destination, startIndex,
std::size_t(long(source.size()) - 1)) /*recursion*/)
return false;
return true;
if (!extract(source, destination, 0, endIndex) /*recursion*/)
return false;
} else {
for (std::size_t i = startIndex; i <= endIndex; ++i) {
destination.append(source[i]);
}
}
} else
return false;
return true;
}
//!
//! \brief Makes the SimpleMissionItem \p item a takeoff command.
//! \param item SimpleMissionItem
//! \param vehilce Vehicle.
//! \return Returns true if successfull, false either.
//!
bool makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle);
bool makeTakeOffCmd(SimpleMissionItem *item,
PlanMasterController *masterController);
//!
//! \brief Makes the SimpleMissionItem \p item a land command.
//! \param item SimpleMissionItem
//! \param vehilce Vehicle.
//! \return Returns true if successfull, false either.
//!
bool makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle);
bool makeLandCmd(SimpleMissionItem *item,
PlanMasterController *masterController);
//!
//! \brief Makes the SimpleMissionItem \p item a MAV_CMD_DO_CHANGE_SPEED item.
......@@ -133,58 +113,56 @@ bool makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle);
//!
void makeSpeedCmd(SimpleMissionItem *item, double speed);
//!
//! \brief Initializes the list \p list.
//!
//! Adds a MissionSettingsItem to \p list.
//! \param list List of VisualMissionItems.
//! \param vehicle Vehicle.
//! \param flyView Fly- or planview.
void initialize(QmlObjectListModel &list,
Vehicle *vehicle,
bool flyView=true);
PlanMasterController *masterController, bool flyView = true);
//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the given index to list.
//! \param coordinate Coordinate of the SimpleMissionItem.
//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the
//! given index to list. \param coordinate Coordinate of the SimpleMissionItem.
//! \param index Index to insert SimpleMissionItem.
//! \param list List of SimpleMissionItems.
//! \param vehicle Vehicle.
//! \param flyView Fly- or planview.
//! \param parent Parent of the SimpleMissionItem (Set to &list if nullptr).
//! \param doUpdates Neccessary update are performed to the list \p list if set to true. Set this to true for the last item only,
//! if inserting multiple items.
//! \return Returns true on success, false either.
//! \param doUpdates Neccessary update are performed to the list \p list if set
//! to true. Set this to true for the last item only, if inserting multiple
//! items. \return Returns true on success, false either.
//!
//! @note The list must be initialized. (\see \fn initialize)
//! @note This function doesn't establish any signal-slot connections (\see \fn MissionController::_initVisualItem).
//! @note This function doesn't set the MissionFlightStatus_t (\see \fn VisualMissionItem::setMissionFlightStatus).
bool insertMissionItem(const QGeoCoordinate &coordinate,
long index,
//! @note This function doesn't establish any signal-slot connections (\see \fn
//! MissionController::_initVisualItem).
//! @note This function doesn't set the MissionFlightStatus_t (\see \fn
//! VisualMissionItem::setMissionFlightStatus).
bool insertMissionItem(const QGeoCoordinate &coordinate, long index,
QmlObjectListModel &list,
Vehicle *vehicle,
bool flyView=true,
QObject *parent=nullptr,
bool doUpdates=true);
PlanMasterController *masterController,
bool flyView = true, QObject *parent = nullptr,
bool doUpdates = true);
//!
//! \brief Performs a list update.
//!
//! Updates the home position of the MissionSettingsItem, the parent child hirarchy and the sequence numbers.
//! \param list List containing VisualMissionItems.
//! \return Returns true if no errors occured, false either.
//! Updates the home position of the MissionSettingsItem, the parent child
//! hirarchy and the sequence numbers. \param list List containing
//! VisualMissionItems. \return Returns true if no errors occured, false either.
bool updateList(QmlObjectListModel &list);
//! @brief Updates the sequence numbers of the VisualMissionItems inside \p list.
//! @brief Updates the sequence numbers of the VisualMissionItems inside \p
//! list.
//!
//! \param list List containing VisualMissionItems.
//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul = 0).
//! \return Returns true if successfull, false either.
//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul =
//! 0). \return Returns true if successfull, false either.
//!
//! \note If the list \p list contains only one VisualMissionItem it's sequence number is
//! set to 0.
//! \note If the list \p list contains only one VisualMissionItem it's sequence
//! number is set to 0.
bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0);
//! @brief Update the parent child item hierarchy of the VisualMissionItems inside \p list.
//! @brief Update the parent child item hierarchy of the VisualMissionItems
//! inside \p list.
//!
//! \param list List containing VisualMissionItems.
//! \return Returns true if successfull, false either.
......@@ -192,7 +170,8 @@ bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0);
//! \note Returns true on success, false either.
bool updateHirarchy(QmlObjectListModel &list);
//! @brief Updates the home position to the first valid coordinate of the VisualMissionItems inside \p list.
//! @brief Updates the home position to the first valid coordinate of the
//! VisualMissionItems inside \p list.
//!
//! \param list List containing VisualMissionItems.
//! \return Returns true if the home position was updated, false either.
......@@ -204,11 +183,9 @@ bool updateHomePosition(QmlObjectListModel &list);
void printList(QmlObjectListModel &list);
namespace detail {
size_t nextSequenceNumber(QmlObjectListModel &list);
bool previousAltitude(QmlObjectListModel &list,
size_t index,
double &altitude,
int &altitudeMode);
size_t nextSequenceNumber(QmlObjectListModel &list);
bool previousAltitude(QmlObjectListModel &list, size_t index, double &altitude,
int &altitudeMode);
} // namespace detail
} // namespace Utils
......
src/Wima/WimaController.cc
View file @ 99e5ae38
......@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T 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
const char *WimaController::areaItemsName = "AreaItems";
......@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
_enableWimaController(settingsGroup,
_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);
_metaDataMap[enableWimaControllerName]){
// PlanData and Progress.
connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this,
&WimaController::planDataChangedHandler);
......@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
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() {
auto &items = _currentWM->currentMissionItems();
if (_masterController && _masterController->managerVehicle() &&
......@@ -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,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) {
......@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
emit missionItemsChanged();
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);
}
}
src/Wima/WimaController.h
View file @ 99e5ae38
......@@ -70,10 +70,6 @@ public:
// Member Methodes
Q_INVOKABLE WimaController *thisPointer();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload();
......@@ -97,9 +93,6 @@ signals:
// Waypoints.
void missionItemsChanged(void);
void waypointPathChanged(void);
// Smart RTL.
void smartRTLRequestConfirm(void);
void smartRTLPathConfirm(void);
// Upload.
void forceUploadConfirm(void);
......@@ -111,11 +104,6 @@ private slots:
QVector<QGeoCoordinate> &path);
void _updateflightSpeed(void);
void _updateAltitude(void);
// SMART RTL
void _checkBatteryLevel(void);
bool _SRTLPrecondition(QString &errorString);
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
// Periodic tasks.
......@@ -152,10 +140,6 @@ private:
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _altitude; // mission altitude
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Periodic tasks.
QTimer _eventTimer;
EventTicker _batteryLevelTicker;
......
src/Wima/WimaPlaner.cc
View file @ 99e5ae38
......@@ -39,9 +39,9 @@ const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent)
: QObject(parent), _masterController(nullptr), _missionController(nullptr),
_currentAreaIndex(-1), _joinedArea(this), _survey(nullptr),
_synchronized(false), _nemoInterface(this),
_stateMachine(new StateMachine), _areasMonitored(false),
_missionControllerMonitored(false), _progressLocked(false) {
_nemoInterface(this), _stateMachine(new StateMachine),
_areasMonitored(false), _missionControllerMonitored(false),
_progressLocked(false), _synchronized(false) {
connect(this, &WimaPlaner::currentPolygonIndexChanged, this,
&WimaPlaner::updatePolygonInteractivity);
......@@ -286,37 +286,43 @@ void WimaPlaner::_update() {
// Check if polygons have at least three vertices
if (_serviceArea.count() < 3) {
qgcApp()->showMessage(tr("Service area has less than three vertices."));
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Service area has less than three vertices."));
return;
}
if (_measurementArea.count() < 3) {
qgcApp()->showMessage(
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"),
tr("Measurement area has less than three vertices."));
return;
}
// Check for simple polygons
if (!_serviceArea.isSimplePolygon()) {
qgcApp()->showMessage(tr("Service area is not a simple polygon. "
"Only simple polygons allowed.\n"));
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Service area is not a simple polygon. Only "
"simple polygons allowed."));
return;
}
if (!_corridor.isSimplePolygon() && _corridor.count() > 0) {
qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only "
"simple polygons allowed.\n"));
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Corridor is not a simple polygon. Only simple "
"polygons allowed."));
return;
}
if (!_measurementArea.isSimplePolygon()) {
qgcApp()->showMessage(tr("Measurement area is not a simple "
"polygon. Only simple polygons allowed.\n"));
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Measurement area is not a simple polygon. Only "
"simple polygons allowed."));
return;
}
if (!_serviceArea.containsCoordinate(_serviceArea.depot())) {
qgcApp()->showMessage(tr("Depot not inside service area."));
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"), tr("Depot not inside service area."));
return;
}
......@@ -326,7 +332,8 @@ void WimaPlaner::_update() {
_joinedArea.join(_corridor);
}
if (!_joinedArea.join(_measurementArea)) {
qgcApp()->showMessage(
qgcApp()->warningMessageBoxOnMainThread(
tr("Area Error"),
tr("Not able to join areas. Service and measurement area"
" must be overlapping, or connected through a "
"corridor."));
......@@ -380,9 +387,9 @@ void WimaPlaner::_update() {
// Remove old arrival and return path.
int size = missionItems->count();
for (int i = surveyIndex + 1; i < size; i++)
_missionController->removeMissionItem(surveyIndex + 1);
_missionController->removeVisualItem(surveyIndex + 1);
for (int i = surveyIndex - 1; i > 1; i--)
_missionController->removeMissionItem(i);
_missionController->removeVisualItem(i);
// set home position to serArea center
MissionSettingsItem *settingsItem =
......@@ -398,20 +405,7 @@ void WimaPlaner::_update() {
settingsItem->setCoordinate(depot);
// set takeoff position
bool setCommandNeeded = false;
if (missionItems->count() < 3) {
setCommandNeeded = true;
_missionController->insertSimpleMissionItem(depot, 1);
}
SimpleMissionItem *takeOffItem =
qobject_cast<SimpleMissionItem *>(missionItems->get(1));
if (takeOffItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): takeOffItem == nullptr";
return;
}
if (setCommandNeeded)
_missionController->setTakeoffCommand(*takeOffItem);
takeOffItem->setCoordinate(depot);
_missionController->insertTakeoffItem(depot, 1, false);
if (_survey->visualTransectPoints().size() == 0) {
qCWarning(WimaPlanerLog) << "update(): survey no points";
......@@ -423,11 +417,10 @@ void WimaPlaner::_update() {
QGeoCoordinate end = _survey->coordinate();
QVector<QGeoCoordinate> path;
if (!shortestPath(start, end, path)) {
qgcApp()->showMessage(
QString(tr("Not able to calculate path from "
"takeoff position to measurement area."))
.toLocal8Bit()
.data());
qgcApp()->warningMessageBoxOnMainThread(
tr("Path Error"), tr("Not able to calculate path from "
"depot position to measurement area. Please "
"double check area and route parameters."));
return;
}
......@@ -441,11 +434,10 @@ void WimaPlaner::_update() {
end = depot;
path.clear();
if (!shortestPath(start, end, path)) {
qgcApp()->showMessage(
QString(tr("Not able to calculate the path from "
"measurement area to landing position."))
.toLocal8Bit()
.data());
qgcApp()->warningMessageBoxOnMainThread(
tr("Path Error"), tr("Not able to calculate path from "
"measurement area to depot position. Please "
"double check area and route parameters."));
return;
}
......@@ -458,16 +450,11 @@ void WimaPlaner::_update() {
(void)_missionController->insertSimpleMissionItem(depot,
missionItems->count());
// create land position item
(void)_missionController->insertSimpleMissionItem(depot,
missionItems->count());
SimpleMissionItem *landItem = qobject_cast<SimpleMissionItem *>(
missionItems->get(missionItems->count() - 1));
if (landItem == nullptr) {
qCWarning(WimaPlanerLog) << "update(): landItem == nullptr";
return;
}
if (!_missionController->setLandCommand(*landItem))
return;
(void)_missionController->insertLandItem(depot, missionItems->count(),
true);
auto surveyIndex = _missionController->visualItems()->indexOf(_survey);
_missionController->setCurrentPlanViewSeqNum(surveyIndex, true);
this->_stateMachine->updateState(EVENT::PATH_UPDATED);
}
......@@ -576,7 +563,8 @@ void WimaPlaner::saveToFile(const QString &filename) {
QFile file(planFilename);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
qgcApp()->showMessage(
qgcApp()->warningMessageBoxOnMainThread(
tr("Save Error"),
tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
_currentFile.clear();
emit currentFileChanged();
......@@ -589,9 +577,11 @@ void WimaPlaner::saveToFile(const QString &filename) {
fileType = FileType::PlanFile;
} else {
if (planFilename.contains(".")) {
qgcApp()->showMessage(tr("File format not supported"));
qgcApp()->warningMessageBoxOnMainThread(
tr("Save Error"), tr("File format not supported"));
} else {
qgcApp()->showMessage(tr("File without file extension not accepted."));
qgcApp()->warningMessageBoxOnMainThread(
tr("Save Error"), tr("File without file extension not accepted."));
return;
}
}
......@@ -642,7 +632,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
errorString = file.errorString() + QStringLiteral(" ") + filename;
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
errorMessage.arg(errorString));
return false;
}
......@@ -651,7 +642,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
QByteArray bytes = file.readAll();
if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
errorMessage.arg(errorString));
return false;
}
......@@ -671,7 +663,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
bool success = _measurementArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
......@@ -683,7 +676,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
bool success = _serviceArea.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
......@@ -695,7 +689,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
bool success = _corridor.loadFromJson(jsonArea, errorString);
if (!success) {
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
......@@ -705,7 +700,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
} else {
errorString +=
QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(
tr("Load Error"), errorMessage.arg(errorString));
return false;
}
} else {
......@@ -776,7 +772,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
return true;
} else {
errorString += QString(tr("File extension not supported.\n"));
qgcApp()->showMessage(errorMessage.arg(errorString));
qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"),
errorMessage.arg(errorString));
return false;
}
}
......
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