Skip to content

Q
qgroundcontrol
Commit 570fbc03 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

code not passing assertions

parent 0a9d836e
Hide whitespace changes
Inline Side-by-side
Showing with 6039 additions and 739 deletions
Paths/ErrorShortestPath.wima 0 → 100644
View file @ 570fbc03
This source diff could not be displayed because it is too large. You can view the blob instead.
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @ 570fbc03
...@@ -301,12 +301,6 @@ Item { ...@@ -301,12 +301,6 @@ Item {
Layout.fillWidth: true Layout.fillWidth: true
} }
FactCheckBox {
text: qsTr("Invert Phase")
fact: wimaController.reverse
Layout.fillWidth: true
}
} }
SectionHeader{ SectionHeader{
......
src/MissionManager/MissionController.cc
View file @ 570fbc03
...@@ -485,7 +485,7 @@ int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, i ...@@ -485,7 +485,7 @@ int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, i
return newItem->sequenceNumber(); return newItem->sequenceNumber();
} }
int MissionController::insertSimpleMissionItem(SimpleMissionItem &missionItem, int i) int MissionController::insertSimpleMissionItem(const SimpleMissionItem &missionItem, int i)
{ {
int sequenceNumber = _nextSequenceNumber(); int sequenceNumber = _nextSequenceNumber();
SimpleMissionItem * newItem = new SimpleMissionItem(missionItem, _flyView, this); SimpleMissionItem * newItem = new SimpleMissionItem(missionItem, _flyView, this);
......
src/MissionManager/MissionController.h
View file @ 570fbc03
...@@ -124,7 +124,7 @@ public: ...@@ -124,7 +124,7 @@ public:
/// Add a new simple mission item to the list /// Add a new simple mission item to the list
/// @param i: index to insert at /// @param i: index to insert at
/// @return Sequence number for new item /// @return Sequence number for new item
int insertSimpleMissionItem(SimpleMissionItem &missionItem, int i); int insertSimpleMissionItem(const SimpleMissionItem &missionItem, int i);
/// Add a new ROI mission item to the list /// Add a new ROI mission item to the list
/// @param i: index to insert at /// @param i: index to insert at
......
src/Wima/Geometry/PolygonCalculus.cc
View file @ 570fbc03
...@@ -479,7 +479,7 @@ namespace PolygonCalculus { ...@@ -479,7 +479,7 @@ namespace PolygonCalculus {
{ {
using namespace PlanimetryCalculus; using namespace PlanimetryCalculus;
QPolygonF bigPolygon(polygon); QPolygonF bigPolygon(polygon);
offsetPolygon(bigPolygon, 0.1); // solves numerical errors offsetPolygon(bigPolygon, 0.5); // solves numerical errors
if ( bigPolygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill) if ( bigPolygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
&& bigPolygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) { && bigPolygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {
......
src/Wima/WaypointManager/DefaultManager.cpp
View file @ 570fbc03
...@@ -4,57 +4,86 @@ ...@@ -4,57 +4,86 @@
#include "MissionSettingsItem.h" #include "MissionSettingsItem.h"
WaypointManager::DefaultManager::DefaultManager(Settings &settings,
AreaInterface &interface)
: ManagerBase(settings)
, _areaInterface(&interface)
{
}
void WaypointManager::DefaultManager::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
bool WaypointManager::DefaultManager::update() bool WaypointManager::DefaultManager::update()
{ {
// extract waypoints // extract waypoints
_slice.clear(); _currentWaypoints.clear();
Slicer::update(_waypoints, _slice); Slicer::update(_waypoints, _currentWaypoints);
return _worker(); return _worker();
} }
bool WaypointManager::DefaultManager::next() bool WaypointManager::DefaultManager::next()
{ {
// extract waypoints // extract waypoints
_slice.clear(); _currentWaypoints.clear();
Slicer::next(_waypoints, _slice); Slicer::next(_waypoints, _currentWaypoints);
return _worker(); return _worker();
} }
bool WaypointManager::DefaultManager::previous() bool WaypointManager::DefaultManager::previous()
{ {
// extract waypoints // extract waypoints
_slice.clear(); _currentWaypoints.clear();
Slicer::previous(_waypoints, _slice); Slicer::previous(_waypoints, _currentWaypoints);
return _worker(); return _worker();
} }
bool WaypointManager::DefaultManager::reset() bool WaypointManager::DefaultManager::reset()
{ {
// extract waypoints // extract waypoints
_slice.clear(); _currentWaypoints.clear();
Slicer::reset(_waypoints, _slice); Slicer::reset(_waypoints, _currentWaypoints);
return _worker(); return _worker();
} }
bool WaypointManager::DefaultManager::_insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate) bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate)
{ {
using namespace WaypointManager::Utils; using namespace WaypointManager::Utils;
if ( !insertMissionItem(c, if ( !insertMissionItem(c,
index /*insertion index*/, index /*insertion index*/,
_missionItems, list,
_settings->vehicle(), _settings->vehicle(),
_settings->isFlyView(), _settings->isFlyView(),
&_missionItems /*parent*/, &_currentMissionItems /*parent*/,
doUpdate /*do update*/) ) doUpdate /*do update*/) )
{ {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed."); qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
return false; return false;
} }
return true; return true;
} }
bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate)
{
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::DefaultManager::_calcShortestPath( bool WaypointManager::DefaultManager::_calcShortestPath(
const QGeoCoordinate &start, const QGeoCoordinate &start,
const QGeoCoordinate &destination, const QGeoCoordinate &destination,
...@@ -77,33 +106,53 @@ bool WaypointManager::DefaultManager::_worker() ...@@ -77,33 +106,53 @@ bool WaypointManager::DefaultManager::_worker()
{ {
using namespace WaypointManager::Utils; using namespace WaypointManager::Utils;
if (_waypoints.count() < 1) { if (_waypoints.count() < 1 || !_settings->valid()) {
return false; return false;
} }
_missionItems.clearAndDeleteContents(); if (_dirty) {
initialize(_missionItems, _settings->vehicle(), _settings->isFlyView()); _missionItems.clearAndDeleteContents();
_waypointsVariant.clear();
initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());
for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
const QGeoCoordinate &c = _waypoints.at(i);
_insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/);
_waypointsVariant.push_back(QVariant::fromValue(c));
}
Q_ASSERT(_missionItems.value<MissionSettingsItem*>(0) != nullptr);
doUpdate(_missionItems);
_dirty = false;
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView());
// calculate path from home to first waypoint // calculate path from home to first waypoint
QVector<QGeoCoordinate> arrivalPath; QVector<QGeoCoordinate> arrivalPath;
if (!_settings->homePosition().isValid()){ if (!_settings->homePosition().isValid()){
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): home position invalid."); qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
return false; return false;
} }
if ( !_calcShortestPath(_settings->homePosition(), _slice.first(), arrivalPath) ) { if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint."); qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
return false; return false;
} }
arrivalPath.removeFirst(); arrivalPath.removeFirst();
// calculate path from last waypoint to home // calculate path from last waypoint to home
qDebug() << "_currentWaypoints.size(): " << _currentWaypoints.size();
qDebug() << "_currentWaypoints.first(): " << _currentWaypoints.first();
qDebug() << "_currentWaypoints.last(): " << _currentWaypoints.last();
qDebug() << "_settings->homePosition(): " << _settings->homePosition();
QVector<QGeoCoordinate> returnPath; QVector<QGeoCoordinate> returnPath;
if ( !_calcShortestPath(_slice.last(), _settings->homePosition(), returnPath) ) { //if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
assert(false); if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), returnPath) ) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint."); qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
return false; //return false;
} }
returnPath.removeFirst(); returnPath.removeFirst();
returnPath.removeLast(); returnPath.removeLast();
...@@ -112,29 +161,29 @@ bool WaypointManager::DefaultManager::_worker() ...@@ -112,29 +161,29 @@ bool WaypointManager::DefaultManager::_worker()
// Create mission items. // Create mission items.
// Set home position of MissionSettingsItem. // Set home position of MissionSettingsItem.
MissionSettingsItem* settingsItem = _missionItems.value<MissionSettingsItem *>(0); MissionSettingsItem* settingsItem = _currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) { if (settingsItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
settingsItem->setCoordinate(_settings->homePosition()); settingsItem->setCoordinate(_settings->homePosition());
// Set takeoff position for first mission item (bug). // Set takeoff position for first mission item (bug).
_insertMissionItem(1 /*insertion index*/, _settings->homePosition(), false /*do update*/); _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, false /*do update*/);
SimpleMissionItem *takeoffItem = _missionItems.value<SimpleMissionItem*>(1); SimpleMissionItem *takeoffItem = _currentMissionItems.value<SimpleMissionItem*>(1);
if (takeoffItem == nullptr) { if (takeoffItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
takeoffItem->setCoordinate(_settings->homePosition()); takeoffItem->setCoordinate(_settings->homePosition());
// Create change speed item. // Create change speed item.
_insertMissionItem(2 /*insertion index*/, _settings->homePosition(), false /*do update*/); _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/);
SimpleMissionItem *speedItem = _missionItems.value<SimpleMissionItem*>(2); SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
if (speedItem == nullptr) { if (speedItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
...@@ -145,57 +194,57 @@ bool WaypointManager::DefaultManager::_worker() ...@@ -145,57 +194,57 @@ bool WaypointManager::DefaultManager::_worker()
// insert arrival path // insert arrival path
for (auto coordinate : arrivalPath) { for (auto coordinate : arrivalPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/, _insertMissionItem(coordinate,
coordinate, _currentMissionItems.count() /*insertion index*/,
false /*do update*/); false /*do update*/);
} }
// Create change speed item (after arrival path). // Create change speed item (after arrival path).
int index = _missionItems.count(); int index = _currentMissionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.first(), false /*do update*/); _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index); speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) { if (speedItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_slice.first()); speedItem->setCoordinate(_currentWaypoints.first());
speedItem->missionItem().setParam2(_settings->flightSpeed()); speedItem->missionItem().setParam2(_settings->flightSpeed());
// Insert slice. // Insert slice.
for (auto coordinate : _slice) { for (auto coordinate : _currentWaypoints) {
_insertMissionItem(_missionItems.count() /*insertion index*/, _insertMissionItem(coordinate,
coordinate, _currentMissionItems.count() /*insertion index*/,
false /*do update*/); false /*do update*/);
} }
// Create change speed item, after slice. // Create change speed item, after slice.
index = _missionItems.count(); index = _currentMissionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.last(), false /*do update*/); _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index); speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) { if (speedItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_slice.last()); speedItem->setCoordinate(_currentWaypoints.last());
speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed()); speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
// Insert return path coordinates. // Insert return path coordinates.
for (auto coordinate : returnPath) { for (auto coordinate : returnPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/, _insertMissionItem(coordinate,
coordinate, _currentMissionItems.count() /*insertion index*/,
false /*do update*/); false /*do update*/);
} }
// Set land command for last mission item. // Set land command for last mission item.
index = _missionItems.count(); index = _currentMissionItems.count();
_insertMissionItem(index /*insertion index*/, _settings->homePosition(), false /*do update*/); _insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/);
SimpleMissionItem *landItem = _missionItems.value<SimpleMissionItem*>(index); SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (landItem == nullptr) { if (landItem == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr."); qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false; return false;
} }
...@@ -204,24 +253,28 @@ bool WaypointManager::DefaultManager::_worker() ...@@ -204,24 +253,28 @@ bool WaypointManager::DefaultManager::_worker()
if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
landItem->setCommand(landCmd); landItem->setCommand(landCmd);
} else { } else {
assert(false); Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Land command not supported!"); qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
return false; return false;
} }
// Prepend arrival path to slice. // Prepend arrival path to slice.
for ( long i = arrivalPath.size()-1; i >=0; --i ) for ( long i = arrivalPath.size()-1; i >=0; --i )
_slice.push_front(arrivalPath[i]); _currentWaypoints.push_front(arrivalPath[i]);
// Append return path to slice. // Append return path to slice.
for ( auto c : returnPath ) for ( auto c : returnPath )
_slice.push_back(c); _currentWaypoints.push_back(c);
_currentWaypointsVariant.clear();
for ( auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
// Set altitude. // Set altitude.
for (int i = 0; i < _missionItems.count(); ++i) { for (int i = 0; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _missionItems.value<SimpleMissionItem *>(i); SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) { if (item == nullptr) {
assert(false); Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr"); qWarning("WimaController::updateAltitude(): nullptr");
return false; return false;
} }
......
src/Wima/WaypointManager/DefaultManager.h
View file @ 570fbc03
...@@ -20,8 +20,10 @@ class DefaultManager : public ManagerBase ...@@ -20,8 +20,10 @@ class DefaultManager : public ManagerBase
{ {
public: public:
DefaultManager() = delete; DefaultManager() = delete;
DefaultManager(Settings *settings, DefaultManager(Settings &settings,
AreaInterface *interface); AreaInterface &interface);
void clear() override;
virtual bool update() override; virtual bool update() override;
...@@ -30,7 +32,13 @@ public: ...@@ -30,7 +32,13 @@ public:
virtual bool reset() override; virtual bool reset() override;
protected: protected:
bool _insertMissionItem(size_t index, const QGeoCoordinate &c, bool doUpdate); bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate);
bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate);
bool _calcShortestPath(const QGeoCoordinate &start, bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination, const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path); QVector<QGeoCoordinate> &path);
......
src/Wima/WaypointManager/GenericWaypointManager.h
View file @ 570fbc03
...@@ -16,21 +16,25 @@ public: ...@@ -16,21 +16,25 @@ public:
typedef ContainerType<WaypointType> WaypointList; typedef ContainerType<WaypointType> WaypointList;
GenericWaypointManager() = delete; GenericWaypointManager() = delete;
GenericWaypointManager(SettingsType *settings); GenericWaypointManager(SettingsType &settings);
// Waypoint editing. // Waypoint editing.
void setWaypoints(const WaypointList &waypoints); void setWaypoints(const WaypointList &waypoints);
void push_back (const WaypointType &wp); void push_back (const WaypointType &wp);
void push_front (const WaypointType &wp); void push_front (const WaypointType &wp);
void clear (); virtual void clear ();
void insert (std::size_t i, const WaypointType &wp); void insert (std::size_t i, const WaypointType &wp);
std::size_t size () const; std::size_t size () const;
WaypointType &at (std::size_t i); WaypointType &at (std::size_t i);
const WaypointList &waypoints() const; const WaypointList &waypoints() const;
const WaypointList &slice() const; const WaypointList &currentWaypoints() const;
const MissionItemList &missionItems() const; const MissionItemList &missionItems() const;
const MissionItemList &currentMissionItems() const;
const QVariantList &waypointsVariant() const;
const QVariantList &currentWaypointsVariant() const;
virtual bool update() = 0; virtual bool update() = 0;
virtual bool next() = 0; virtual bool next() = 0;
...@@ -39,9 +43,13 @@ public: ...@@ -39,9 +43,13 @@ public:
protected: protected:
WaypointList _waypoints; WaypointList _waypoints;
WaypointList _slice; WaypointList _currentWaypoints;
MissionItemList _currentMissionItems;
MissionItemList _missionItems; MissionItemList _missionItems;
SettingsType *_settings; SettingsType *_settings;
bool _dirty;
QVariantList _waypointsVariant;
QVariantList _currentWaypointsVariant;
}; };
...@@ -52,9 +60,10 @@ template<class WaypointType, ...@@ -52,9 +60,10 @@ template<class WaypointType,
GenericWaypointManager<WaypointType, GenericWaypointManager<WaypointType,
ContainerType, ContainerType,
MissionItemList, MissionItemList,
SettingsType>::GenericWaypointManager(SettingsType *Settings SettingsType>::GenericWaypointManager(SettingsType &settings)
) : Slicer()
: _settings(_settings) , _settings(&settings)
, _dirty(true)
{} {}
template<class WaypointType, template<class WaypointType,
...@@ -66,6 +75,7 @@ void GenericWaypointManager<WaypointType, ...@@ -66,6 +75,7 @@ void GenericWaypointManager<WaypointType,
MissionItemList, MissionItemList,
SettingsType>::push_back(const WaypointType &wp) SettingsType>::push_back(const WaypointType &wp)
{ {
_dirty = true;
_waypoints.push_back(wp); _waypoints.push_back(wp);
} }
...@@ -79,6 +89,7 @@ void GenericWaypointManager<WaypointType, ...@@ -79,6 +89,7 @@ void GenericWaypointManager<WaypointType,
SettingsType>::push_front( SettingsType>::push_front(
const WaypointType &wp) const WaypointType &wp)
{ {
_dirty = true;
_waypoints.push_front(wp); _waypoints.push_front(wp);
} }
...@@ -91,7 +102,13 @@ void GenericWaypointManager<WaypointType, ...@@ -91,7 +102,13 @@ void GenericWaypointManager<WaypointType,
MissionItemList, MissionItemList,
SettingsType>::clear() SettingsType>::clear()
{ {
_dirty = true;
_waypoints.clear(); _waypoints.clear();
_currentWaypoints.clear();
_missionItems.clear();
_currentMissionItems.clear();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
} }
template<class WaypointType, template<class WaypointType,
...@@ -104,6 +121,7 @@ void GenericWaypointManager<WaypointType, ...@@ -104,6 +121,7 @@ void GenericWaypointManager<WaypointType,
SettingsType>::insert(std::size_t i, SettingsType>::insert(std::size_t i,
const WaypointType &wp) const WaypointType &wp)
{ {
_dirty = true;
_waypoints.insert(i, wp); _waypoints.insert(i, wp);
} }
...@@ -131,6 +149,30 @@ WaypointType & GenericWaypointManager<WaypointType, ...@@ -131,6 +149,30 @@ WaypointType & GenericWaypointManager<WaypointType,
return _waypoints.at(i); return _waypoints.at(i);
} }
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::waypointsVariant() const
{
return _waypointsVariant;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentWaypointsVariant() const
{
return _currentWaypointsVariant;
}
template<class WaypointType, template<class WaypointType,
template<class, class...> class ContainerType, template<class, class...> class ContainerType,
class MissionItemList, class MissionItemList,
...@@ -150,9 +192,9 @@ template<class WaypointType, ...@@ -150,9 +192,9 @@ template<class WaypointType,
const ContainerType<WaypointType> &GenericWaypointManager<WaypointType, const ContainerType<WaypointType> &GenericWaypointManager<WaypointType,
ContainerType, ContainerType,
MissionItemList, MissionItemList,
SettingsType>::slice() const SettingsType>::currentWaypoints() const
{ {
return _slice; return _currentWaypoints;
} }
template<class WaypointType, template<class WaypointType,
...@@ -167,6 +209,18 @@ const MissionItemList &GenericWaypointManager<WaypointType, ...@@ -167,6 +209,18 @@ const MissionItemList &GenericWaypointManager<WaypointType,
return _missionItems; return _missionItems;
} }
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const MissionItemList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentMissionItems() const
{
return _currentMissionItems;
}
template<class WaypointType, template<class WaypointType,
template<class, class...> class ContainerType, template<class, class...> class ContainerType,
class MissionItemList, class MissionItemList,
......
src/Wima/WaypointManager/Settings.cpp
View file @ 570fbc03
#include "Settings.h" #include "Settings.h"
void WaypointManager::Settings::setHomePosition(QGeoCoordinate &c) WaypointManager::Settings::Settings()
: _missionController(nullptr)
, _isFlyView(true)
, _arrivalReturnSpeed(5)
, _flightSpeed(2)
, _altitude(5)
{
}
bool WaypointManager::Settings::valid() const
{
return _missionController != nullptr
&& _homePosition.isValid();
}
void WaypointManager::Settings::setHomePosition(const QGeoCoordinate &c)
{ {
_homePosition = c; _homePosition = c;
} }
void WaypointManager::Settings::setVehicle(Vehicle *vehicle)
void WaypointManager::Settings::setMissionController(MissionController *controller)
{
_missionController = controller;
}
void WaypointManager::Settings::setMasterController(PlanMasterController *controller)
{ {
_vehicle = vehicle; _masterController = controller;
} }
void WaypointManager::Settings::setIsFlyView(bool isFlyView) void WaypointManager::Settings::setIsFlyView(bool isFlyView)
...@@ -37,9 +59,24 @@ const QGeoCoordinate &WaypointManager::Settings::homePosition() const ...@@ -37,9 +59,24 @@ const QGeoCoordinate &WaypointManager::Settings::homePosition() const
return _homePosition; return _homePosition;
} }
QGeoCoordinate &WaypointManager::Settings::homePosition()
{
return _homePosition;
}
MissionController *WaypointManager::Settings::missionController() const
{
return _missionController;
}
PlanMasterController *WaypointManager::Settings::masterController() const
{
return _masterController;
}
Vehicle *WaypointManager::Settings::vehicle() const Vehicle *WaypointManager::Settings::vehicle() const
{ {
return _vehicle; return _masterController->managerVehicle();
} }
bool WaypointManager::Settings::isFlyView() const bool WaypointManager::Settings::isFlyView() const
......
src/Wima/WaypointManager/Settings.h
View file @ 570fbc03
...@@ -2,6 +2,8 @@ ...@@ -2,6 +2,8 @@
#include <QGeoCoordinate> #include <QGeoCoordinate>
#include "MissionController.h"
#include "PlanMasterController.h"
#include "Vehicle.h" #include "Vehicle.h"
namespace WaypointManager { namespace WaypointManager {
...@@ -11,14 +13,20 @@ class Settings ...@@ -11,14 +13,20 @@ class Settings
public: public:
Settings(); Settings();
void setHomePosition(QGeoCoordinate &c); bool valid() const;
void setVehicle(Vehicle *vehicle);
void setHomePosition(const QGeoCoordinate &c);
void setMissionController(MissionController *controller);
void setMasterController(PlanMasterController *controller);
void setIsFlyView(bool isFlyView); void setIsFlyView(bool isFlyView);
void setArrivalReturnSpeed(double speed); void setArrivalReturnSpeed(double speed);
void setFlightSpeed(double speed); void setFlightSpeed(double speed);
void setAltitude(double altitude); void setAltitude(double altitude);
const QGeoCoordinate &homePosition() const; const QGeoCoordinate &homePosition() const;
QGeoCoordinate &homePosition();
MissionController *missionController() const;
PlanMasterController *masterController() const;
Vehicle *vehicle() const; Vehicle *vehicle() const;
bool isFlyView() const; bool isFlyView() const;
double arrivalReturnSpeed() const; double arrivalReturnSpeed() const;
...@@ -26,12 +34,13 @@ public: ...@@ -26,12 +34,13 @@ public:
double altitude() const; double altitude() const;
private: private:
QGeoCoordinate _homePosition; QGeoCoordinate _homePosition;
Vehicle *_vehicle; MissionController *_missionController;
bool _isFlyView; PlanMasterController *_masterController;
double _arrivalReturnSpeed; bool _isFlyView;
double _flightSpeed; double _arrivalReturnSpeed;
double _altitude; double _flightSpeed;
double _altitude;
}; };
......
src/Wima/WaypointManager/Slicer.h
View file @ 570fbc03
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
#include "Utils.h" #include "Utils.h"
//! @brief Base class for all waypoint managers. //! @brief Helper class for slicing containers.
class Slicer class Slicer
{ {
public: public:
...@@ -67,14 +67,13 @@ private: ...@@ -67,14 +67,13 @@ private:
template <class Container1, class Container2> template <class Container1, class Container2>
void Slicer::update(const Container1 &source, Container2 &slice){ void Slicer::update(const Container1 &source, Container2 &slice){
if ( !_idxValid) _updateIdx(source.size());
_updateIdx(source.size());
WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd); WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd);
} }
template <class Container1, class Container2> template <class Container1, class Container2>
void Slicer::next(const Container1 &source, Container2 &slice){ void Slicer::next(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxNext); setStartIndex(_idxNext);
update(source, slice); update(source, slice);
} }
...@@ -82,6 +81,7 @@ void Slicer::next(const Container1 &source, Container2 &slice){ ...@@ -82,6 +81,7 @@ void Slicer::next(const Container1 &source, Container2 &slice){
template <class Container1, class Container2> template <class Container1, class Container2>
void Slicer::previous(const Container1 &source, Container2 &slice){ void Slicer::previous(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxPrevious); setStartIndex(_idxPrevious);
update(source, slice); update(source, slice);
} }
......
src/Wima/WaypointManager/Utils.cpp
View file @ 570fbc03
...@@ -48,7 +48,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, ...@@ -48,7 +48,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
double altitude; double altitude;
int altitudeMode; int altitudeMode;
if (detail::previousAltitude(list, --index, altitude, altitudeMode)) { if (detail::previousAltitude(list, index-1, altitude, altitudeMode)) {
newItem->altitude()->setRawValue(altitude); newItem->altitude()->setRawValue(altitude);
newItem->setAltitudeMode( newItem->setAltitudeMode(
static_cast<QGroundControlQmlGlobal::AltitudeMode>( static_cast<QGroundControlQmlGlobal::AltitudeMode>(
...@@ -153,10 +153,12 @@ bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list) ...@@ -153,10 +153,12 @@ bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list)
bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list) bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list)
{ {
MissionSettingsItem *settingsItem = list.value<MissionSettingsItem *>(0); MissionSettingsItem *settingsItem = list.value<MissionSettingsItem *>(0);
assert(settingsItem); // list not initialized?
// Set the home position to be a delta from first coordinate // Set the home position to be a delta from first coordinate.
for (int i = 1; i < list.count(); ++i) { for (int i = 1; i < list.count(); ++i) {
VisualMissionItem* item = list.value<VisualMissionItem*>(i); VisualMissionItem* item = list.value<VisualMissionItem*>(i);
assert(item);
if (item->specifiesCoordinate() && item->coordinate().isValid()) { if (item->specifiesCoordinate() && item->coordinate().isValid()) {
QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(30, 0); QGeoCoordinate c = item->coordinate().atDistanceAndAzimuth(30, 0);
......
src/Wima/WaypointManager/Utils.h
View file @ 570fbc03
...@@ -86,7 +86,7 @@ bool extract(const ContainerType<CoordinateType> &source, ...@@ -86,7 +86,7 @@ bool extract(const ContainerType<CoordinateType> &source,
if ( !extract(source, if ( !extract(source,
destination, destination,
startIndex, startIndex,
std::size_t(source.size()-1)) /*recursion*/) std::size_t(long(source.size())-1)) /*recursion*/)
return false; return false;
if ( !extract(source, if ( !extract(source,
destination, destination,
......
src/Wima/WimaController.cc
View file @ 570fbc03
...@@ -30,7 +30,6 @@ const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionIte ...@@ -30,7 +30,6 @@ const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionIte
const char* WimaController::flightSpeedName = "FlightSpeed"; const char* WimaController::flightSpeedName = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed"; const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char* WimaController::altitudeName = "Altitude"; const char* WimaController::altitudeName = "Altitude";
const char* WimaController::reverseName = "Reverse";
const char* WimaController::snakeTileWidthName = "SnakeTileWidth"; const char* WimaController::snakeTileWidthName = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName = "SnakeTileHeight"; const char* WimaController::snakeTileHeightName = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
...@@ -43,10 +42,10 @@ using namespace snake_geometry; ...@@ -43,10 +42,10 @@ using namespace snake_geometry;
WimaController::WimaController(QObject *parent) WimaController::WimaController(QObject *parent)
: QObject (parent) : QObject (parent)
, _container (nullptr) , _container (nullptr)
, _joinedArea (this) , _joinedArea ()
, _measurementArea (this) , _measurementArea ()
, _serviceArea (this) , _serviceArea ()
, _corridor (this) , _corridor ()
, _localPlanDataValid (false) , _localPlanDataValid (false)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
...@@ -58,9 +57,11 @@ WimaController::WimaController(QObject *parent) ...@@ -58,9 +57,11 @@ WimaController::WimaController(QObject *parent)
, _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName])
, _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
, _altitude (settingsGroup, _metaDataMap[altitudeName]) , _altitude (settingsGroup, _metaDataMap[altitudeName])
, _reverse (settingsGroup, _metaDataMap[reverseName]) , _areaInterface(&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _endWaypointIndex (0) , _managerSettings()
, _startWaypointIndex (0) , _defaultManager(_managerSettings, _areaInterface)
, _snakeManager(_managerSettings, _areaInterface)
, _currentManager(_defaultManager)
, _uploadOverrideRequired (false) , _uploadOverrideRequired (false)
, _measurementPathLength (-1) , _measurementPathLength (-1)
, _arrivalPathLength (-1) , _arrivalPathLength (-1)
...@@ -82,15 +83,19 @@ WimaController::WimaController(QObject *parent) ...@@ -82,15 +83,19 @@ WimaController::WimaController(QObject *parent)
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _pRosBridge (new ROSBridge::ROSBridge()) , _pRosBridge (new ROSBridge::ROSBridge())
{ {
// Set up facts.
_showAllMissionItems.setRawValue(true); _showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true); _showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateNextWaypoint); connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_recalcCurrentPhase); connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed); connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed); connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
connect(&_reverse, &Fact::rawValueChanged, this, &WimaController::_reverseChangedHandler);
_defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
_defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
_defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());
// setup low battery handling // setup low battery handling
connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
...@@ -120,15 +125,25 @@ MissionController *WimaController::missionController() { ...@@ -120,15 +125,25 @@ MissionController *WimaController::missionController() {
} }
QmlObjectListModel *WimaController::visualItems() { QmlObjectListModel *WimaController::visualItems() {
return &_visualItems; return &_areas;
}
QmlObjectListModel *WimaController::missionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager.missionItems());
}
QmlObjectListModel *WimaController::currentMissionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager.currentMissionItems());
}
QVariantList WimaController::waypointPath()
{
return const_cast<QVariantList&>(_currentManager.waypointsVariant());
} }
QVariantList WimaController::waypointPath() const QVariantList WimaController::currentWaypointPath()
{ {
QVariantList list; return const_cast<QVariantList&>(_currentManager.currentWaypointsVariant());
for ( auto wp : _waypoints)
list.append(QVariant::fromValue(wp));
return list;
} }
//QStringList WimaController::loadNameFilters() const //QStringList WimaController::loadNameFilters() const
...@@ -181,12 +196,14 @@ bool WimaController::snakeCalcInProgress() const ...@@ -181,12 +196,14 @@ bool WimaController::snakeCalcInProgress() const
void WimaController::setMasterController(PlanMasterController *masterC) void WimaController::setMasterController(PlanMasterController *masterC)
{ {
_masterController = masterC; _masterController = masterC;
_managerSettings.setMasterController(masterC);
emit masterControllerChanged(); emit masterControllerChanged();
} }
void WimaController::setMissionController(MissionController *missionC) void WimaController::setMissionController(MissionController *missionC)
{ {
_missionController = missionC; _missionController = missionC;
_managerSettings.setMissionController(missionC);
emit missionControllerChanged(); emit missionControllerChanged();
} }
...@@ -225,41 +242,34 @@ void WimaController::nextPhase() ...@@ -225,41 +242,34 @@ void WimaController::nextPhase()
} }
void WimaController::previousPhase() void WimaController::previousPhase()
{ {
bool reverseBool = _reverse.rawValue().toBool(); if ( !_currentManager.previous() ) {
if (!reverseBool){ assert(false);
int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt();
if (startIndex > 0) {
_nextPhaseStartWaypointIndex.setRawValue(1+std::max(_startWaypointIndex
- _maxWaypointsPerPhase.rawValue().toInt()
+ _overlapWaypoints.rawValue().toInt(), 0));
}
}
else {
int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt();
if (startIndex <= _missionItems.count()) {
_nextPhaseStartWaypointIndex.setRawValue(1+std::min(_startWaypointIndex
+ _maxWaypointsPerPhase.rawValue().toInt()
- _overlapWaypoints.rawValue().toInt(), _missionItems.count()-1));
}
} }
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
} }
void WimaController::resetPhase() void WimaController::resetPhase()
{ {
bool reverseBool = _reverse.rawValue().toBool(); if ( !_currentManager.reset() ) {
if (!reverseBool) { assert(false);
_nextPhaseStartWaypointIndex.setRawValue(int(1));
}
else {
_nextPhaseStartWaypointIndex.setRawValue(_missionItems.count());
} }
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
} }
bool WimaController::uploadToVehicle() bool WimaController::uploadToVehicle()
{ {
auto &currentMissionItems = _defaultManager.currentMissionItems();
if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate()) if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
&& _currentMissionItems.count() > 0) { && currentMissionItems.count() > 0) {
setUploadOverrideRequired(true); setUploadOverrideRequired(true);
return false; return false;
} }
...@@ -269,8 +279,9 @@ bool WimaController::uploadToVehicle() ...@@ -269,8 +279,9 @@ bool WimaController::uploadToVehicle()
bool WimaController::forceUploadToVehicle() bool WimaController::forceUploadToVehicle()
{ {
auto &currentMissionItems = _defaultManager.currentMissionItems();
setUploadOverrideRequired(false); setUploadOverrideRequired(false);
if (_currentMissionItems.count() < 1) if (currentMissionItems.count() < 1)
return false; return false;
_missionController->removeAll(); _missionController->removeAll();
...@@ -278,30 +289,16 @@ bool WimaController::forceUploadToVehicle() ...@@ -278,30 +289,16 @@ bool WimaController::forceUploadToVehicle()
QmlObjectListModel* visuals = _missionController->visualItems(); QmlObjectListModel* visuals = _missionController->visualItems();
MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0); MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) { if (settingsItem == nullptr) {
assert(false);
qWarning("WimaController::updateCurrentMissionItems(): nullptr"); qWarning("WimaController::updateCurrentMissionItems(): nullptr");
return false; return false;
} }
settingsItem->setCoordinate(_takeoffLandPostion); settingsItem->setCoordinate(_managerSettings.homePosition());
// copy mission items from _currentMissionItems to _missionController // Copy mission items to _missionController.
for (int i = 0; i < _currentMissionItems.count(); i++){ for (int i = 0; i < currentMissionItems.count(); i++){
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i); auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count()); _missionController->insertSimpleMissionItem(*item, visuals->count());
if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
}
}
for (int i = 0; i < _missionController->visualItems()->count(); i++){
SimpleMissionItem *item = _missionController->visualItems()->value<SimpleMissionItem*>(i);
if (item == nullptr)
continue;
if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
}
}
for (int i = 0; i < _missionController->visualItems()->count(); i++){
SimpleMissionItem *item = _missionController->visualItems()->value<SimpleMissionItem*>(i);
if (item == nullptr)
continue;
} }
_masterController->sendToVehicle(); _masterController->sendToVehicle();
...@@ -329,12 +326,12 @@ bool WimaController::checkSmartRTLPreCondition() ...@@ -329,12 +326,12 @@ bool WimaController::checkSmartRTLPreCondition()
bool WimaController::calcReturnPath() bool WimaController::calcReturnPath()
{ {
QString errorString; QString errorString;
bool retValue = _calcReturnPath(errorString); // bool retValue = _calcReturnPath(errorString);
if (retValue == false) { // if (retValue == false) {
qgcApp()->showMessage(errorString); // qgcApp()->showMessage(errorString);
return false; // return false;
} // }
return true; // return true;
} }
void WimaController::executeSmartRTL() void WimaController::executeSmartRTL()
...@@ -408,11 +405,8 @@ bool WimaController::_fetchContainerData() ...@@ -408,11 +405,8 @@ bool WimaController::_fetchContainerData()
// fetch only if valid, return true on success // fetch only if valid, return true on success
// reset visual items // reset visual items
_visualItems.clear(); _areas.clear();
_missionItems.clearAndDeleteContents(); _defaultManager.clear();
_currentMissionItems.clearAndDeleteContents();
_waypoints.clear();
_currentWaypointPath.clear();
_snakeTiles.polygons().clear(); _snakeTiles.polygons().clear();
_snakeTilesLocal.polygons().clear(); _snakeTilesLocal.polygons().clear();
_snakeTileCenterPoints.clear(); _snakeTileCenterPoints.clear();
...@@ -420,6 +414,7 @@ bool WimaController::_fetchContainerData() ...@@ -420,6 +414,7 @@ bool WimaController::_fetchContainerData()
emit visualItemsChanged(); emit visualItemsChanged();
emit missionItemsChanged(); emit missionItemsChanged();
emit currentMissionItemsChanged(); emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged(); emit currentWaypointPathChanged();
emit snakeTilesChanged(); emit snakeTilesChanged();
emit snakeTileCenterPointsChanged(); emit snakeTileCenterPointsChanged();
...@@ -428,6 +423,7 @@ bool WimaController::_fetchContainerData() ...@@ -428,6 +423,7 @@ bool WimaController::_fetchContainerData()
if (_container == nullptr) { if (_container == nullptr) {
qWarning("WimaController::fetchContainerData(): No container assigned!"); qWarning("WimaController::fetchContainerData(): No container assigned!");
assert(false);
return false; return false;
} }
...@@ -437,14 +433,14 @@ bool WimaController::_fetchContainerData() ...@@ -437,14 +433,14 @@ bool WimaController::_fetchContainerData()
QList<const WimaAreaData*> areaList = planData.areaList(); QList<const WimaAreaData*> areaList = planData.areaList();
int areaCounter = 0; int areaCounter = 0;
int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
for (int i = 0; i < areaList.size(); i++) { for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i]; const WimaAreaData *areaData = areaList[i];
if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area? if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData); _serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
areaCounter++; areaCounter++;
_visualItems.append(&_serviceArea); _areas.append(&_serviceArea);
continue; continue;
} }
...@@ -452,7 +448,7 @@ bool WimaController::_fetchContainerData() ...@@ -452,7 +448,7 @@ bool WimaController::_fetchContainerData()
if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area? if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea = *qobject_cast<const WimaMeasurementAreaData*>(areaData); _measurementArea = *qobject_cast<const WimaMeasurementAreaData*>(areaData);
areaCounter++; areaCounter++;
_visualItems.append(&_measurementArea); _areas.append(&_measurementArea);
continue; continue;
} }
...@@ -468,7 +464,7 @@ bool WimaController::_fetchContainerData() ...@@ -468,7 +464,7 @@ bool WimaController::_fetchContainerData()
if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor? if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast<const WimaJoinedAreaData*>(areaData); _joinedArea = *qobject_cast<const WimaJoinedAreaData*>(areaData);
areaCounter++; areaCounter++;
_visualItems.append(&_joinedArea); _areas.append(&_joinedArea);
continue; continue;
} }
...@@ -477,45 +473,29 @@ bool WimaController::_fetchContainerData() ...@@ -477,45 +473,29 @@ bool WimaController::_fetchContainerData()
break; break;
} }
// extract mission items if (areaCounter != numAreas) {
QList<MissionItem> tempMissionItems = planData.missionItems(); assert(false);
if (tempMissionItems.size() < 1)
return false; return false;
// create mission items
_missionController->removeAll();
QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
// create SimpleMissionItem by using _missionController
for ( int i = 0; i < tempMissionItems.size(); i++) {
_missionController->insertSimpleMissionItem(tempMissionItems[i], missionControllerVisualItems->count());
} }
// copy mission items from _missionController to _missionItems
for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
SimpleMissionItem *visualItem = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
if (visualItem == nullptr) {
qWarning("WimaController::fetchContainerData(): Nullptr at SimpleMissionItem!");
return false;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
_missionItems.append(visualItemCopy);
}
if (areaCounter != numAreas)
return false;
if (!_setTakeoffLandPosition()) // extract mission items
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1) {
assert(false);
return false; return false;
}
_updateWaypointPath(); for (auto item : tempMissionItems)
_defaultManager.push_back(item.coordinate());
// set _nextPhaseStartWaypointIndex to 1 _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); _serviceArea.center().longitude(),
bool reverse = _reverse.rawValue().toBool(); 0) );
_nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
if(!_calcNextPhase()) if( !_defaultManager.update() ){
assert(false);
return false; return false;
}
// Initialize _scenario. // Initialize _scenario.
Area mArea; Area mArea;
...@@ -545,6 +525,7 @@ bool WimaController::_fetchContainerData() ...@@ -545,6 +525,7 @@ bool WimaController::_fetchContainerData()
// Check if scenario is defined. // Check if scenario is defined.
if ( !_verifyScenarioDefinedWithErrorMessage() ) if ( !_verifyScenarioDefinedWithErrorMessage() )
assert(false);
return false; return false;
{ {
...@@ -585,8 +566,11 @@ bool WimaController::_fetchContainerData() ...@@ -585,8 +566,11 @@ bool WimaController::_fetchContainerData()
} }
emit visualItemsChanged(); emit visualItemsChanged();
emit missionItemsChanged(); emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged(); emit snakeTilesChanged();
emit snakeTileCenterPointsChanged(); emit snakeTileCenterPointsChanged();
...@@ -596,325 +580,102 @@ bool WimaController::_fetchContainerData() ...@@ -596,325 +580,102 @@ bool WimaController::_fetchContainerData()
bool WimaController::_calcNextPhase() bool WimaController::_calcNextPhase()
{ {
if (_missionItems.count() < 1) { bool value;
_startWaypointIndex = 0; _currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value));
_endWaypointIndex = 0; Q_ASSERT(value);
return false; (void)value;
}
_currentMissionItems.clearAndDeleteContents();
bool reverse = _reverse.rawValue().toBool(); // Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
int startIndex = _nextPhaseStartWaypointIndex.rawValue().toInt()-1;
if (!reverse) {
if (startIndex > _missionItems.count()-1)
return false;
}
else {
if (startIndex < 0)
return false;
}
_startWaypointIndex = startIndex;
int maxWaypointsPerPhase = _maxWaypointsPerPhase.rawValue().toInt();
// determine end waypoint index
bool lastMissionPhaseReached = false;
if (!reverse) {
_endWaypointIndex = std::min(_startWaypointIndex + maxWaypointsPerPhase - 1, _missionItems.count()-1);
if (_endWaypointIndex == _missionItems.count() - 1)
lastMissionPhaseReached = true;
}
else {
_endWaypointIndex = std::max(_startWaypointIndex - maxWaypointsPerPhase + 1, 0);
if (_endWaypointIndex == 0)
lastMissionPhaseReached = true;
}
// extract waypoints
QVector<QGeoCoordinate> CSWpList; // list with potential waypoints (from _missionItems), for _currentMissionItems
if (!reverse) { if ( !_currentManager.next() ) {
if (!getCoordinates(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) { assert(false);
qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
return false;
}
} else {
if (!getCoordinates(_missionItems, CSWpList, _endWaypointIndex, _startWaypointIndex)) {
qWarning("WimaController::calcNextPhase(): error on waypoint extraction.");
return false;
}
// reverse path
QVector<QGeoCoordinate> reversePath;
for (QGeoCoordinate c : CSWpList)
reversePath.prepend(c);
CSWpList.clear();
CSWpList.append(reversePath);
}
// calculate phase length
_measurementPathLength = 0;
for (int i = 0; i < CSWpList.size()-1; ++i)
_measurementPathLength += CSWpList[i].distanceTo(CSWpList[i+1]);
// set start waypoint index for next phase
if (!lastMissionPhaseReached) {
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
if (!reverse) {
int untruncated = std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0);
int truncated = std::min(untruncated , _missionItems.count()-1);
_nextPhaseStartWaypointIndex.setRawValue(truncated + 1);
}
else {
int untruncated = std::min(_endWaypointIndex - 1 + _overlapWaypoints.rawValue().toInt(), _missionItems.count()-1);
int truncated = std::max(untruncated , 0);
_nextPhaseStartWaypointIndex.setRawValue(truncated + 1);
}
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
}
// calculate path from home to first waypoint
QVector<QGeoCoordinate> arrivalPath;
if (!_takeoffLandPostion.isValid()){
qWarning("WimaController::calcNextPhase(): _takeoffLandPostion not valid.");
return false;
}
if ( !calcShortestPath(_takeoffLandPostion, CSWpList.first(), arrivalPath) ) {
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
return false;
}
// calculate arrival path length
_arrivalPathLength = 0;
for (int i = 0; i < arrivalPath.size()-1; ++i)
_arrivalPathLength += arrivalPath[i].distanceTo(arrivalPath[i+1]);
arrivalPath.removeFirst();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) {
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to last waypoint.");
return false;
}
// calculate arrival path length
_returnPathLength = 0;
for (int i = 0; i < returnPath.size()-1; ++i)
_returnPathLength += returnPath[i].distanceTo(returnPath[i+1]);
returnPath.removeFirst();
returnPath.removeLast();
// create Mission Items
_missionController->removeAll();
QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();
// set homeposition of settingsItem
MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
settingsItem->setCoordinate(_takeoffLandPostion);
// set takeoff position for first mission item (bug)
missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1);
SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1);
if (takeoffItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
takeoffItem->setCoordinate(_takeoffLandPostion);
// create change speed item, after take off
_missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
if (speedItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false; return false;
} }
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);// set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_takeoffLandPostion);
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// insert arrival path
for (auto coordinate : arrivalPath)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// create change speed item, after arrival path
int index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.first(), index);
speedItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(CSWpList.first());
speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
// insert Circular Survey coordinates
for (auto coordinate : CSWpList)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// create change speed item, after circular survey
index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.last(), index);
speedItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(CSWpList.last());
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// insert return path coordinates
for (auto coordinate : returnPath)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// set land command for last mission item
index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(_takeoffLandPostion, index);
SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(index);
if (landItem == nullptr) {
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
_missionController->setLandCommand(*landItem);
// copy to _currentMissionItems
for ( int i = 1; i < missionControllerVisuals->count(); i++) {
SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i);
if (visualItem == nullptr) {
qWarning("WimaController::calcNextPhase(): Nullptr at SimpleMissionItem!");
_currentMissionItems.clear();
return false;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
_currentMissionItems.append(visualItemCopy);
}
double dist = 0; emit missionItemsChanged();
double time = 0;
if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
_setPhaseDistance(dist);
_setPhaseDuration(time);
_missionController->removeAll(); // remove items from _missionController, will be added on upload
_updateAltitude();
_updateCurrentPath();
emit currentMissionItemsChanged(); emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true; return true;
} }
void WimaController::_updateWaypointPath() void WimaController::_recalcCurrentPhase()
{ {
_waypoints.clear(); if ( !_currentManager.update() ) {
getCoordinates(_missionItems, assert(false);
_waypoints, }
0,
std::size_t( std::max(_missionItems.count()-1,0) )
);
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged(); emit waypointPathChanged();
} }
void WimaController::_updateCurrentPath()
void WimaController::_updateOverlap()
{ {
_currentWaypointPath.clear(); bool value;
getCoordinates(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1); _currentManager.setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager.update() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged(); emit currentWaypointPathChanged();
emit waypointPathChanged();
} }
void WimaController::_updateNextWaypoint() void WimaController::_updateMaxWaypoints()
{ {
if (_endWaypointIndex < _missionItems.count()-2) { bool value;
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); _currentManager.setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
_nextPhaseStartWaypointIndex.setRawValue(1 + std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0)); Q_ASSERT(value);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); (void)value;
}
}
void WimaController::_recalcCurrentPhase() if ( !_currentManager.update() ) {
{ assert(false);
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); }
_nextPhaseStartWaypointIndex.setRawValue(_startWaypointIndex + 1);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_calcNextPhase();
}
bool WimaController::_setTakeoffLandPosition() emit missionItemsChanged();
{ emit currentMissionItemsChanged();
_takeoffLandPostion.setAltitude(0); emit currentWaypointPathChanged();
_takeoffLandPostion.setLongitude(_serviceArea.center().longitude()); emit waypointPathChanged();
_takeoffLandPostion.setLatitude(_serviceArea.center().latitude());
return true;
} }
void WimaController::_updateflightSpeed() void WimaController::_updateflightSpeed()
{ {
int speedItemCounter = 0; _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble());
for (int i = 0; i < _currentMissionItems.count(); i++) { _currentManager.update();
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) {
speedItemCounter++;
if (speedItemCounter == 2) {
item->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
}
}
}
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble() emit missionItemsChanged();
+ (_arrivalPathLength + _returnPathLength) emit currentMissionItemsChanged();
/ _arrivalReturnSpeed.rawValue().toDouble()); emit currentWaypointPathChanged();
emit waypointPathChanged();
if (speedItemCounter != 3)
qWarning("WimaController::updateflightSpeed(): internal error.");
} }
void WimaController::_updateArrivalReturnSpeed() void WimaController::_updateArrivalReturnSpeed()
{ {
int speedItemCounter = 0; _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble());
for (int i = 0; i < _currentMissionItems.count(); i++) { _currentManager.update();
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) {
speedItemCounter++;
if (speedItemCounter != 2) {
item->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
}
}
}
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
if (speedItemCounter != 3)
qWarning("WimaController::updateArrivalReturnSpeed(): internal error.");
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
} }
void WimaController::_updateAltitude() void WimaController::_updateAltitude()
{ {
for (int i = 0; i < _currentMissionItems.count(); i++) { _managerSettings.setAltitude(_altitude.rawValue().toDouble());
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i); _currentManager.update();
if (item == nullptr) {
qWarning("WimaController::updateAltitude(): nullptr"); emit missionItemsChanged();
return; emit currentMissionItemsChanged();
} emit currentWaypointPathChanged();
item->altitude()->setRawValue(_altitude.rawValue().toDouble()); emit waypointPathChanged();
}
} }
void WimaController::_checkBatteryLevel() void WimaController::_checkBatteryLevel()
...@@ -986,18 +747,18 @@ void WimaController::_eventTimerHandler() ...@@ -986,18 +747,18 @@ void WimaController::_eventTimerHandler()
void WimaController::_smartRTLCleanUp(bool flying) void WimaController::_smartRTLCleanUp(bool flying)
{ {
if ( !flying) { // vehicle has landed // if ( !flying) { // vehicle has landed
if (_executingSmartRTL) { // if (_executingSmartRTL) {
_executingSmartRTL = false; // _executingSmartRTL = false;
_loadCurrentMissionItemsFromBuffer(); // _loadCurrentMissionItemsFromBuffer();
_setPhaseDistance(_phaseDistanceBuffer); // _setPhaseDistance(_phaseDistanceBuffer);
_setPhaseDuration(_phaseDurationBuffer); // _setPhaseDuration(_phaseDurationBuffer);
_showAllMissionItems.setRawValue(true); // _showAllMissionItems.setRawValue(true);
_missionController->removeAllFromVehicle(); // _missionController->removeAllFromVehicle();
_missionController->removeAll(); // _missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); // disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
} // }
} // }
} }
void WimaController::_enableDisableLowBatteryHandling(QVariant enable) void WimaController::_enableDisableLowBatteryHandling(QVariant enable)
...@@ -1009,15 +770,6 @@ void WimaController::_enableDisableLowBatteryHandling(QVariant enable) ...@@ -1009,15 +770,6 @@ void WimaController::_enableDisableLowBatteryHandling(QVariant enable)
} }
} }
void WimaController::_reverseChangedHandler()
{
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_nextPhaseStartWaypointIndex.setRawValue(_endWaypointIndex+1);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_calcNextPhase();
}
void WimaController::_setPhaseDistance(double distance) void WimaController::_setPhaseDistance(double distance)
{ {
if (!qFuzzyCompare(distance, _phaseDistance)) { if (!qFuzzyCompare(distance, _phaseDistance)) {
...@@ -1056,119 +808,6 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString) ...@@ -1056,119 +808,6 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
return true; return true;
} }
bool WimaController::_calcReturnPath(QString &errorSring)
{
// it is assumed that checkSmartRTLPreCondition() was called first and true was returned
Vehicle *managerVehicle = masterController()->managerVehicle();
QGeoCoordinate currentVehiclePosition = managerVehicle->coordinate();
// check if vehicle inside _joinedArea, this statement is not inside checkSmartRTLPreCondition() because during checkSmartRTLPreCondition() vehicle is not paused yet
if (!_joinedArea.containsCoordinate(currentVehiclePosition)) {
errorSring.append(tr("Vehicle not inside joined area. Action not supported."));
return false;
}
// calculate return path
QVector<QGeoCoordinate> returnPath;
calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath);
// successful?
if (returnPath.isEmpty()) {
errorSring.append(tr("Not able to calculate return path."));
return false;
}
// qWarning() << "returnPath.size()" << returnPath.size();
_saveCurrentMissionItemsToBuffer();
_phaseDistanceBuffer = _phaseDistance;
_phaseDurationBuffer = _phaseDuration;
// create Mission Items
removeFromVehicle();
QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();
// set homeposition of settingsItem
MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
settingsItem->setCoordinate(_takeoffLandPostion);
// copy from returnPath to _missionController
QGeoCoordinate speedItemCoordinate = returnPath.first();
for (auto coordinate : returnPath) {
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
}
//qWarning() << "missionControllerVisuals->count()" << missionControllerVisuals->count();
// create speed item
int speedItemIndex = 1;
_missionController->insertSimpleMissionItem(speedItemCoordinate, speedItemIndex);
SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(speedItemIndex);
if (speedItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->setCoordinate(speedItemCoordinate);
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// set second item command to ordinary waypoint (is takeoff)
SimpleMissionItem *secondItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
if (secondItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
secondItem->setCoordinate(speedItemCoordinate);
secondItem->setCommand(MAV_CMD_NAV_WAYPOINT);
// set land command for last mission item
SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
if (landItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
_missionController->setLandCommand(*landItem);
// copy to _currentMissionItems
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
for ( int i = 1; i < missionControllerVisuals->count(); i++) {
SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i);
if (visualItem == nullptr) {
qWarning("WimaController: Nullptr at SimpleMissionItem!");
_currentMissionItems.clear();
return false;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
_currentMissionItems.append(visualItemCopy);
}
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
double dist = 0;
double time = 0;
if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
_setPhaseDistance(dist);
_setPhaseDuration(time);
_missionController->removeAll(); // remove items from _missionController, will be added on upload
_updateAltitude();
_updateCurrentPath();
emit currentMissionItemsChanged();
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
_showAllMissionItems.setRawValue(false);
managerVehicle->trajectoryPoints()->clear();
return true;
}
void WimaController::_setVehicleHasLowBattery(bool batteryLow) void WimaController::_setVehicleHasLowBattery(bool batteryLow)
{ {
if (_vehicleHasLowBattery != batteryLow) { if (_vehicleHasLowBattery != batteryLow) {
...@@ -1180,36 +819,36 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow) ...@@ -1180,36 +819,36 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow)
void WimaController::_initSmartRTL() void WimaController::_initSmartRTL()
{ {
QString errorString; // QString errorString;
static int attemptCounter = 0; // static int attemptCounter = 0;
attemptCounter++; // attemptCounter++;
if (_checkSmartRTLPreCondition(errorString) == true) { // if (_checkSmartRTLPreCondition(errorString) == true) {
_masterController->managerVehicle()->pauseVehicle(); // _masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); // connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
if (_calcReturnPath(errorString)) { // if (_calcReturnPath(errorString)) {
_executingSmartRTL = true; // _executingSmartRTL = true;
attemptCounter = 0; // attemptCounter = 0;
switch(_srtlReason) { // switch(_srtlReason) {
case BatteryLow: // case BatteryLow:
emit returnBatteryLowConfirmRequired(); // emit returnBatteryLowConfirmRequired();
break; // break;
case UserRequest: // case UserRequest:
emit returnUserRequestConfirmRequired(); // emit returnUserRequestConfirmRequired();
break; // break;
} // }
return; // return;
} // }
} // }
if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area // if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area
errorString.append(tr("Smart RTL: No success after maximum number of attempts.")); // errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString); // qgcApp()->showMessage(errorString);
attemptCounter = 0; // attemptCounter = 0;
} else { // } else {
_smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL); // _smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
} // }
} }
void WimaController::_executeSmartRTL() void WimaController::_executeSmartRTL()
...@@ -1255,7 +894,8 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage() ...@@ -1255,7 +894,8 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage()
void WimaController::_snakeStoreWorkerResults() void WimaController::_snakeStoreWorkerResults()
{ {
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
_missionItems.clearAndDeleteContents(); _snakeManager.clear();
const WorkerResult_t &r = _snakeWorker.getResult(); const WorkerResult_t &r = _snakeWorker.getResult();
_setSnakeCalcInProgress(false); _setSnakeCalcInProgress(false);
if (!r.success) { if (!r.success) {
...@@ -1266,56 +906,20 @@ void WimaController::_snakeStoreWorkerResults() ...@@ -1266,56 +906,20 @@ void WimaController::_snakeStoreWorkerResults()
// create Mission items from r.waypoints // create Mission items from r.waypoints
long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2; long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
assert(n >= 1); assert(n >= 1);
QVector<MissionItem> missionItems;
missionItems.reserve(int(n));
// Remove all items from mission controller.
_missionController->removeAll();
QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
// Create QVector<QGeoCoordinate> containing all waypoints; // Create QVector<QGeoCoordinate> containing all waypoints;
unsigned long startIdx = r.arrivalPathIdx.last(); unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first(); unsigned long endIdx = r.returnPathIdx.first();
QVector<QGeoCoordinate> waypoints;
for (unsigned long i = startIdx; i <= endIdx; ++i) { for (unsigned long i = startIdx; i <= endIdx; ++i) {
QGeoCoordinate wp{r.waypoints[int(i)].value<QGeoCoordinate>()}; _snakeManager.push_back(r.waypoints[int(i)].value<QGeoCoordinate>());
waypoints.append(wp);
}
// create SimpleMissionItem by using _missionController
long insertIdx = missionControllerVisualItems->count();
for (auto wp : waypoints)
_missionController->insertSimpleMissionItem(wp, insertIdx++);
SimpleMissionItem *takeOffItem = missionControllerVisualItems->value<SimpleMissionItem*>(1);
if (takeOffItem == nullptr) {
qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
return;
}
takeOffItem->setCommand(MAV_CMD_NAV_WAYPOINT);
takeOffItem->setCoordinate(waypoints[0]);
// copy mission items from _missionController to _missionItems
for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
SimpleMissionItem *visualItem = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
if (visualItem == nullptr) {
qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!");
return;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
_missionItems.append(visualItemCopy);
} }
_updateWaypointPath(); _snakeManager.update();
// set _nextPhaseStartWaypointIndex to 1
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
bool reverse = _reverse.rawValue().toBool();
_nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_calcNextPhase();
emit missionItemsChanged(); emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
auto end = std::chrono::high_resolution_clock::now(); auto end = std::chrono::high_resolution_clock::now();
double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count(); double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
...@@ -1352,25 +956,6 @@ void WimaController::_initStartSnakeWorker() ...@@ -1352,25 +956,6 @@ void WimaController::_initStartSnakeWorker()
_snakeWorker.start(); _snakeWorker.start();
} }
void WimaController::_loadCurrentMissionItemsFromBuffer()
{
_currentMissionItems.clear();
int numItems = _missionItemsBuffer.count();
for (int i = 0; i < numItems; i++)
_currentMissionItems.append(_missionItemsBuffer.removeAt(0));
_updateCurrentPath();
}
void WimaController::_saveCurrentMissionItemsToBuffer()
{
_missionItemsBuffer.clear();
int numCurrentMissionItems = _currentMissionItems.count();
for (int i = 0; i < numCurrentMissionItems; i++)
_missionItemsBuffer.append(_currentMissionItems.removeAt(0));
}
void WimaController::_progressFromJson(JsonDocUPtr pDoc, void WimaController::_progressFromJson(JsonDocUPtr pDoc,
QNemoProgress &progress) QNemoProgress &progress)
{ {
......
src/Wima/WimaController.h
View file @ 570fbc03
...@@ -35,6 +35,8 @@ ...@@ -35,6 +35,8 @@
#include "ros_bridge/include/ROSBridge.h" #include "ros_bridge/include/ROSBridge.h"
#include "WaypointManager/DefaultManager.h"
#define CHECK_BATTERY_INTERVAL 1000 // ms #define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
...@@ -136,10 +138,6 @@ public: ...@@ -136,10 +138,6 @@ public:
READ arrivalReturnSpeed READ arrivalReturnSpeed
CONSTANT CONSTANT
) )
Q_PROPERTY(Fact* reverse
READ reverse
CONSTANT
)
Q_PROPERTY(bool uploadOverrideRequired Q_PROPERTY(bool uploadOverrideRequired
READ uploadOverrideRequired READ uploadOverrideRequired
WRITE setUploadOverrideRequired WRITE setUploadOverrideRequired
...@@ -216,10 +214,10 @@ public: ...@@ -216,10 +214,10 @@ public:
// QString fileExtension (void) const { return wimaFileExtension; } // QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon joinedArea (void) const; QGCMapPolygon joinedArea (void) const;
WimaDataContainer* dataContainer (void) { return _container; } WimaDataContainer* dataContainer (void) { return _container; }
QmlObjectListModel* missionItems (void) { return &_missionItems; } QmlObjectListModel* missionItems (void);
QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; } QmlObjectListModel* currentMissionItems (void);
QVariantList waypointPath (void) const; QVariantList waypointPath (void);
QVariantList currentWaypointPath (void) { return _currentWaypointPath; } QVariantList currentWaypointPath (void);
Fact* enableWimaController (void) { return &_enableWimaController; } Fact* enableWimaController (void) { return &_enableWimaController; }
Fact* overlapWaypoints (void) { return &_overlapWaypoints; } Fact* overlapWaypoints (void) { return &_overlapWaypoints; }
Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; } Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; }
...@@ -229,7 +227,6 @@ public: ...@@ -229,7 +227,6 @@ public:
Fact* flightSpeed (void) { return &_flightSpeed; } Fact* flightSpeed (void) { return &_flightSpeed; }
Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; } Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; }
Fact* altitude (void) { return &_altitude; } Fact* altitude (void) { return &_altitude; }
Fact* reverse (void) { return &_reverse; }
Fact* enableSnake (void) { return &_enableSnake; } Fact* enableSnake (void) { return &_enableSnake; }
Fact* snakeTileWidth (void) { return &_snakeTileWidth;} Fact* snakeTileWidth (void) { return &_snakeTileWidth;}
...@@ -291,7 +288,6 @@ public: ...@@ -291,7 +288,6 @@ public:
static const char* flightSpeedName; static const char* flightSpeedName;
static const char* arrivalReturnSpeedName; static const char* arrivalReturnSpeedName;
static const char* altitudeName; static const char* altitudeName;
static const char* reverseName;
static const char* snakeTileWidthName; static const char* snakeTileWidthName;
static const char* snakeTileHeightName; static const char* snakeTileHeightName;
static const char* snakeMinTileAreaName; static const char* snakeMinTileAreaName;
...@@ -330,11 +326,13 @@ private: ...@@ -330,11 +326,13 @@ private:
private slots: private slots:
bool _fetchContainerData(); bool _fetchContainerData();
bool _calcNextPhase(void); bool _calcNextPhase(void);
void _updateWaypointPath (void); //void _updateWaypointPath (void);
void _updateCurrentPath (void); //void _updateCurrentPath (void);
void _updateNextWaypoint (void); //void _updateNextWaypoint (void);
void _recalcCurrentPhase (void); void _recalcCurrentPhase (void);
bool _setTakeoffLandPosition (void); //bool _setTakeoffLandPosition (void);
void _updateOverlap (void);
void _updateMaxWaypoints (void);
void _updateflightSpeed (void); void _updateflightSpeed (void);
void _updateArrivalReturnSpeed (void); void _updateArrivalReturnSpeed (void);
void _updateAltitude (void); void _updateAltitude (void);
...@@ -342,7 +340,6 @@ private slots: ...@@ -342,7 +340,6 @@ private slots:
void _eventTimerHandler (void); void _eventTimerHandler (void);
void _smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL void _smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL
void _enableDisableLowBatteryHandling (QVariant enable); void _enableDisableLowBatteryHandling (QVariant enable);
void _reverseChangedHandler ();
void _initSmartRTL (); void _initSmartRTL ();
void _executeSmartRTL (); void _executeSmartRTL ();
void _setSnakeConnectionStatus (SnakeConnectionStatus status); void _setSnakeConnectionStatus (SnakeConnectionStatus status);
...@@ -369,19 +366,27 @@ private: ...@@ -369,19 +366,27 @@ private:
MissionController *_missionController; MissionController *_missionController;
// QString _currentFile; // file for saveing // QString _currentFile; // file for saveing
WimaDataContainer *_container; // container for data exchange with WimaController WimaDataContainer *_container; // container for data exchange with WimaController
QmlObjectListModel _visualItems; // contains all visible areas QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid; bool _localPlanDataValid;
QmlObjectListModel _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView
QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
// _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage WaypointManager::AreaInterface _areaInterface;
QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked WaypointManager::Settings _managerSettings;
QVector<QGeoCoordinate> _waypoints; // path connecting the items in _missionItems WaypointManager::DefaultManager _defaultManager;
QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems WaypointManager::DefaultManager _snakeManager;
QGeoCoordinate _takeoffLandPostion; WaypointManager::ManagerBase &_currentManager;
// QmlObjectListModel _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView
// QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
// // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage
// QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked
// QVector<QGeoCoordinate> _waypoints; // path connecting the items in _missionItems
// QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems
// QGeoCoordinate _takeoffLandPostion;
QMap<QString, FactMetaData*> _metaDataMap; QMap<QString, FactMetaData*> _metaDataMap;
...@@ -395,13 +400,12 @@ private: ...@@ -395,13 +400,12 @@ private:
SettingsFact _flightSpeed; // mission flight speed SettingsFact _flightSpeed; // mission flight speed
SettingsFact _arrivalReturnSpeed; // arrival and return path speed SettingsFact _arrivalReturnSpeed; // arrival and return path speed
SettingsFact _altitude; // mission altitude SettingsFact _altitude; // mission altitude
SettingsFact _reverse; // Reverses the phase direction. Phases go from high to low waypoint numbers, if true.
SettingsFact _enableSnake; // Enable Snake (see snake.h) SettingsFact _enableSnake; // Enable Snake (see snake.h)
int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element // int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element
// (which is not part of the return path) of _currentMissionItem // // (which is not part of the return path) of _currentMissionItem
int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element // int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element
// (which is not part of the arrival path) of _currentMissionItem // // (which is not part of the arrival path) of _currentMissionItem
bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area.
// The user can override the upload lock with a slider, this will reset this variable to false. // The user can override the upload lock with a slider, this will reset this variable to false.
double _measurementPathLength; // the lenght of the phase in meters double _measurementPathLength; // the lenght of the phase in meters
......
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