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 {
Layout.fillWidth: true
}
FactCheckBox {
text: qsTr("Invert Phase")
fact: wimaController.reverse
Layout.fillWidth: true
}
}
SectionHeader{
......
src/MissionManager/MissionController.cc
View file @ 570fbc03
......@@ -485,7 +485,7 @@ int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, i
return newItem->sequenceNumber();
}
int MissionController::insertSimpleMissionItem(SimpleMissionItem &missionItem, int i)
int MissionController::insertSimpleMissionItem(const SimpleMissionItem &missionItem, int i)
{
int sequenceNumber = _nextSequenceNumber();
SimpleMissionItem * newItem = new SimpleMissionItem(missionItem, _flyView, this);
......
src/MissionManager/MissionController.h
View file @ 570fbc03
......@@ -124,7 +124,7 @@ public:
/// Add a new simple mission item to the list
/// @param i: index to insert at
/// @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
/// @param i: index to insert at
......
src/Wima/Geometry/PolygonCalculus.cc
View file @ 570fbc03
......@@ -479,7 +479,7 @@ namespace PolygonCalculus {
{
using namespace PlanimetryCalculus;
QPolygonF bigPolygon(polygon);
offsetPolygon(bigPolygon, 0.1); // solves numerical errors
offsetPolygon(bigPolygon, 0.5); // solves numerical errors
if ( bigPolygon.containsPoint(startVertex, Qt::FillRule::OddEvenFill)
&& bigPolygon.containsPoint(endVertex, Qt::FillRule::OddEvenFill)) {
......
src/Wima/WaypointManager/DefaultManager.cpp
View file @ 570fbc03
......@@ -4,57 +4,86 @@
#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()
{
// extract waypoints
_slice.clear();
Slicer::update(_waypoints, _slice);
_currentWaypoints.clear();
Slicer::update(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::next()
{
// extract waypoints
_slice.clear();
Slicer::next(_waypoints, _slice);
_currentWaypoints.clear();
Slicer::next(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::previous()
{
// extract waypoints
_slice.clear();
Slicer::previous(_waypoints, _slice);
_currentWaypoints.clear();
Slicer::previous(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::reset()
{
// extract waypoints
_slice.clear();
Slicer::reset(_waypoints, _slice);
_currentWaypoints.clear();
Slicer::reset(_waypoints, _currentWaypoints);
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;
if ( !insertMissionItem(c,
index /*insertion index*/,
_missionItems,
list,
_settings->vehicle(),
_settings->isFlyView(),
&_missionItems /*parent*/,
&_currentMissionItems /*parent*/,
doUpdate /*do update*/) )
{
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
return false;
}
return true;
}
bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate)
{
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::DefaultManager::_calcShortestPath(
const QGeoCoordinate &start,
const QGeoCoordinate &destination,
......@@ -77,33 +106,53 @@ bool WaypointManager::DefaultManager::_worker()
{
using namespace WaypointManager::Utils;
if (_waypoints.count() < 1) {
if (_waypoints.count() < 1 || !_settings->valid()) {
return false;
}
_missionItems.clearAndDeleteContents();
initialize(_missionItems, _settings->vehicle(), _settings->isFlyView());
if (_dirty) {
_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
QVector<QGeoCoordinate> arrivalPath;
if (!_settings->homePosition().isValid()){
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
return false;
}
if ( !_calcShortestPath(_settings->homePosition(), _slice.first(), arrivalPath) ) {
assert(false);
if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
return false;
}
arrivalPath.removeFirst();
// 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;
if ( !_calcShortestPath(_slice.last(), _settings->homePosition(), returnPath) ) {
assert(false);
//if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
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.");
return false;
//return false;
}
returnPath.removeFirst();
returnPath.removeLast();
......@@ -112,29 +161,29 @@ bool WaypointManager::DefaultManager::_worker()
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem* settingsItem = _missionItems.value<MissionSettingsItem *>(0);
MissionSettingsItem* settingsItem = _currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// Set takeoff position for first mission item (bug).
_insertMissionItem(1 /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *takeoffItem = _missionItems.value<SimpleMissionItem*>(1);
_insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, false /*do update*/);
SimpleMissionItem *takeoffItem = _currentMissionItems.value<SimpleMissionItem*>(1);
if (takeoffItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
takeoffItem->setCoordinate(_settings->homePosition());
// Create change speed item.
_insertMissionItem(2 /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *speedItem = _missionItems.value<SimpleMissionItem*>(2);
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/);
SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
if (speedItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
......@@ -145,57 +194,57 @@ bool WaypointManager::DefaultManager::_worker()
// insert arrival path
for (auto coordinate : arrivalPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item (after arrival path).
int index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.first(), false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index);
int index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
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());
// Insert slice.
for (auto coordinate : _slice) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
for (auto coordinate : _currentWaypoints) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item, after slice.
index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _slice.last(), false /*do update*/);
speedItem = _missionItems.value<SimpleMissionItem*>(index);
index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
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());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(_missionItems.count() /*insertion index*/,
coordinate,
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Set land command for last mission item.
index = _missionItems.count();
_insertMissionItem(index /*insertion index*/, _settings->homePosition(), false /*do update*/);
SimpleMissionItem *landItem = _missionItems.value<SimpleMissionItem*>(index);
index = _currentMissionItems.count();
_insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/);
SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (landItem == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
......@@ -204,24 +253,28 @@ bool WaypointManager::DefaultManager::_worker()
if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
landItem->setCommand(landCmd);
} else {
assert(false);
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
return false;
}
// Prepend arrival path to slice.
for ( long i = arrivalPath.size()-1; i >=0; --i )
_slice.push_front(arrivalPath[i]);
_currentWaypoints.push_front(arrivalPath[i]);
// Append return path to slice.
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.
for (int i = 0; i < _missionItems.count(); ++i) {
SimpleMissionItem *item = _missionItems.value<SimpleMissionItem *>(i);
for (int i = 0; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
assert(false);
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
......
src/Wima/WaypointManager/DefaultManager.h
View file @ 570fbc03
......@@ -20,8 +20,10 @@ class DefaultManager : public ManagerBase
{
public:
DefaultManager() = delete;
DefaultManager(Settings *settings,
AreaInterface *interface);
DefaultManager(Settings &settings,
AreaInterface &interface);
void clear() override;
virtual bool update() override;
......@@ -30,7 +32,13 @@ public:
virtual bool reset() override;
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,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
......
src/Wima/WaypointManager/GenericWaypointManager.h
View file @ 570fbc03
......@@ -16,21 +16,25 @@ public:
typedef ContainerType<WaypointType> WaypointList;
GenericWaypointManager() = delete;
GenericWaypointManager(SettingsType *settings);
GenericWaypointManager(SettingsType &settings);
// Waypoint editing.
void setWaypoints(const WaypointList &waypoints);
void push_back (const WaypointType &wp);
void push_front (const WaypointType &wp);
void clear ();
virtual void clear ();
void insert (std::size_t i, const WaypointType &wp);
std::size_t size () const;
WaypointType &at (std::size_t i);
const WaypointList &waypoints() const;
const WaypointList &slice() const;
const WaypointList &currentWaypoints() const;
const MissionItemList &missionItems() const;
const MissionItemList &currentMissionItems() const;
const QVariantList &waypointsVariant() const;
const QVariantList &currentWaypointsVariant() const;
virtual bool update() = 0;
virtual bool next() = 0;
......@@ -39,9 +43,13 @@ public:
protected:
WaypointList _waypoints;
WaypointList _slice;
WaypointList _currentWaypoints;
MissionItemList _currentMissionItems;
MissionItemList _missionItems;
SettingsType *_settings;
bool _dirty;
QVariantList _waypointsVariant;
QVariantList _currentWaypointsVariant;
};
......@@ -52,9 +60,10 @@ template<class WaypointType,
GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::GenericWaypointManager(SettingsType *Settings
)
: _settings(_settings)
SettingsType>::GenericWaypointManager(SettingsType &settings)
: Slicer()
, _settings(&settings)
, _dirty(true)
{}
template<class WaypointType,
......@@ -66,6 +75,7 @@ void GenericWaypointManager<WaypointType,
MissionItemList,
SettingsType>::push_back(const WaypointType &wp)
{
_dirty = true;
_waypoints.push_back(wp);
}
......@@ -79,6 +89,7 @@ void GenericWaypointManager<WaypointType,
SettingsType>::push_front(
const WaypointType &wp)
{
_dirty = true;
_waypoints.push_front(wp);
}
......@@ -91,7 +102,13 @@ void GenericWaypointManager<WaypointType,
MissionItemList,
SettingsType>::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clear();
_currentMissionItems.clear();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
template<class WaypointType,
......@@ -104,6 +121,7 @@ void GenericWaypointManager<WaypointType,
SettingsType>::insert(std::size_t i,
const WaypointType &wp)
{
_dirty = true;
_waypoints.insert(i, wp);
}
......@@ -131,6 +149,30 @@ WaypointType & GenericWaypointManager<WaypointType,
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, class...> class ContainerType,
class MissionItemList,
......@@ -150,9 +192,9 @@ template<class WaypointType,
const ContainerType<WaypointType> &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::slice() const
SettingsType>::currentWaypoints() const
{
return _slice;
return _currentWaypoints;
}
template<class WaypointType,
......@@ -167,6 +209,18 @@ const MissionItemList &GenericWaypointManager<WaypointType,
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, class...> class ContainerType,
class MissionItemList,
......
src/Wima/WaypointManager/Settings.cpp
View file @ 570fbc03
#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;
}
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)
......@@ -37,9 +59,24 @@ const QGeoCoordinate &WaypointManager::Settings::homePosition() const
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
{
return _vehicle;
return _masterController->managerVehicle();
}
bool WaypointManager::Settings::isFlyView() const
......
src/Wima/WaypointManager/Settings.h
View file @ 570fbc03
......@@ -2,6 +2,8 @@
#include <QGeoCoordinate>
#include "MissionController.h"
#include "PlanMasterController.h"
#include "Vehicle.h"
namespace WaypointManager {
......@@ -11,14 +13,20 @@ class Settings
public:
Settings();
void setHomePosition(QGeoCoordinate &c);
void setVehicle(Vehicle *vehicle);
bool valid() const;
void setHomePosition(const QGeoCoordinate &c);
void setMissionController(MissionController *controller);
void setMasterController(PlanMasterController *controller);
void setIsFlyView(bool isFlyView);
void setArrivalReturnSpeed(double speed);
void setFlightSpeed(double speed);
void setAltitude(double altitude);
const QGeoCoordinate &homePosition() const;
QGeoCoordinate &homePosition();
MissionController *missionController() const;
PlanMasterController *masterController() const;
Vehicle *vehicle() const;
bool isFlyView() const;
double arrivalReturnSpeed() const;
......@@ -26,12 +34,13 @@ public:
double altitude() const;
private:
QGeoCoordinate _homePosition;
Vehicle *_vehicle;
bool _isFlyView;
double _arrivalReturnSpeed;
double _flightSpeed;
double _altitude;
QGeoCoordinate _homePosition;
MissionController *_missionController;
PlanMasterController *_masterController;
bool _isFlyView;
double _arrivalReturnSpeed;
double _flightSpeed;
double _altitude;
};
......
src/Wima/WaypointManager/Slicer.h
View file @ 570fbc03
......@@ -5,7 +5,7 @@
#include "Utils.h"
//! @brief Base class for all waypoint managers.
//! @brief Helper class for slicing containers.
class Slicer
{
public:
......@@ -67,14 +67,13 @@ private:
template <class Container1, class Container2>
void Slicer::update(const Container1 &source, Container2 &slice){
if ( !_idxValid)
_updateIdx(source.size());
_updateIdx(source.size());
WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd);
}
template <class Container1, class Container2>
void Slicer::next(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxNext);
update(source, slice);
}
......@@ -82,6 +81,7 @@ void Slicer::next(const Container1 &source, Container2 &slice){
template <class Container1, class Container2>
void Slicer::previous(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxPrevious);
update(source, slice);
}
......
src/Wima/WaypointManager/Utils.cpp
View file @ 570fbc03
......@@ -48,7 +48,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
double altitude;
int altitudeMode;
if (detail::previousAltitude(list, --index, altitude, altitudeMode)) {
if (detail::previousAltitude(list, index-1, altitude, altitudeMode)) {
newItem->altitude()->setRawValue(altitude);
newItem->setAltitudeMode(
static_cast<QGroundControlQmlGlobal::AltitudeMode>(
......@@ -153,10 +153,12 @@ bool WaypointManager::Utils::detail::updateHirarchy(QmlObjectListModel &list)
bool WaypointManager::Utils::detail::updateHomePosition(QmlObjectListModel &list)
{
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) {
VisualMissionItem* item = list.value<VisualMissionItem*>(i);
assert(item);
if (item->specifiesCoordinate() && item->coordinate().isValid()) {
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,
if ( !extract(source,
destination,
startIndex,
std::size_t(source.size()-1)) /*recursion*/)
std::size_t(long(source.size())-1)) /*recursion*/)
return false;
if ( !extract(source,
destination,
......
src/Wima/WimaController.cc
View file @ 570fbc03
......@@ -30,7 +30,6 @@ const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionIte
const char* WimaController::flightSpeedName = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char* WimaController::altitudeName = "Altitude";
const char* WimaController::reverseName = "Reverse";
const char* WimaController::snakeTileWidthName = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
......@@ -43,10 +42,10 @@ using namespace snake_geometry;
WimaController::WimaController(QObject *parent)
: QObject (parent)
, _container (nullptr)
, _joinedArea (this)
, _measurementArea (this)
, _serviceArea (this)
, _corridor (this)
, _joinedArea ()
, _measurementArea ()
, _serviceArea ()
, _corridor ()
, _localPlanDataValid (false)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
......@@ -58,9 +57,11 @@ WimaController::WimaController(QObject *parent)
, _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName])
, _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
, _altitude (settingsGroup, _metaDataMap[altitudeName])
, _reverse (settingsGroup, _metaDataMap[reverseName])
, _endWaypointIndex (0)
, _startWaypointIndex (0)
, _areaInterface(&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _managerSettings()
, _defaultManager(_managerSettings, _areaInterface)
, _snakeManager(_managerSettings, _areaInterface)
, _currentManager(_defaultManager)
, _uploadOverrideRequired (false)
, _measurementPathLength (-1)
, _arrivalPathLength (-1)
......@@ -82,15 +83,19 @@ WimaController::WimaController(QObject *parent)
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _pRosBridge (new ROSBridge::ROSBridge())
{
// Set up facts.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateNextWaypoint);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_recalcCurrentPhase);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
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
connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
......@@ -120,15 +125,25 @@ MissionController *WimaController::missionController() {
}
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;
for ( auto wp : _waypoints)
list.append(QVariant::fromValue(wp));
return list;
return const_cast<QVariantList&>(_currentManager.currentWaypointsVariant());
}
//QStringList WimaController::loadNameFilters() const
......@@ -181,12 +196,14 @@ bool WimaController::snakeCalcInProgress() const
void WimaController::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
_managerSettings.setMasterController(masterC);
emit masterControllerChanged();
}
void WimaController::setMissionController(MissionController *missionC)
{
_missionController = missionC;
_managerSettings.setMissionController(missionC);
emit missionControllerChanged();
}
......@@ -225,41 +242,34 @@ void WimaController::nextPhase()
}
void WimaController::previousPhase()
{
bool reverseBool = _reverse.rawValue().toBool();
if (!reverseBool){
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));
}
{
if ( !_currentManager.previous() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::resetPhase()
{
bool reverseBool = _reverse.rawValue().toBool();
if (!reverseBool) {
_nextPhaseStartWaypointIndex.setRawValue(int(1));
}
else {
_nextPhaseStartWaypointIndex.setRawValue(_missionItems.count());
if ( !_currentManager.reset() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
bool WimaController::uploadToVehicle()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
&& _currentMissionItems.count() > 0) {
&& currentMissionItems.count() > 0) {
setUploadOverrideRequired(true);
return false;
}
......@@ -269,8 +279,9 @@ bool WimaController::uploadToVehicle()
bool WimaController::forceUploadToVehicle()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
setUploadOverrideRequired(false);
if (_currentMissionItems.count() < 1)
if (currentMissionItems.count() < 1)
return false;
_missionController->removeAll();
......@@ -278,30 +289,16 @@ bool WimaController::forceUploadToVehicle()
QmlObjectListModel* visuals = _missionController->visualItems();
MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
assert(false);
qWarning("WimaController::updateCurrentMissionItems(): nullptr");
return false;
}
settingsItem->setCoordinate(_takeoffLandPostion);
settingsItem->setCoordinate(_managerSettings.homePosition());
// copy mission items from _currentMissionItems to _missionController
for (int i = 0; i < _currentMissionItems.count(); i++){
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
// Copy mission items to _missionController.
for (int i = 0; i < currentMissionItems.count(); i++){
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_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();
......@@ -329,12 +326,12 @@ bool WimaController::checkSmartRTLPreCondition()
bool WimaController::calcReturnPath()
{
QString errorString;
bool retValue = _calcReturnPath(errorString);
if (retValue == false) {
qgcApp()->showMessage(errorString);
return false;
}
return true;
// bool retValue = _calcReturnPath(errorString);
// if (retValue == false) {
// qgcApp()->showMessage(errorString);
// return false;
// }
// return true;
}
void WimaController::executeSmartRTL()
......@@ -408,11 +405,8 @@ bool WimaController::_fetchContainerData()
// fetch only if valid, return true on success
// reset visual items
_visualItems.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypoints.clear();
_currentWaypointPath.clear();
_areas.clear();
_defaultManager.clear();
_snakeTiles.polygons().clear();
_snakeTilesLocal.polygons().clear();
_snakeTileCenterPoints.clear();
......@@ -420,6 +414,7 @@ bool WimaController::_fetchContainerData()
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
......@@ -428,6 +423,7 @@ bool WimaController::_fetchContainerData()
if (_container == nullptr) {
qWarning("WimaController::fetchContainerData(): No container assigned!");
assert(false);
return false;
}
......@@ -437,14 +433,14 @@ bool WimaController::_fetchContainerData()
QList<const WimaAreaData*> areaList = planData.areaList();
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++) {
const WimaAreaData *areaData = areaList[i];
if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
areaCounter++;
_visualItems.append(&_serviceArea);
_areas.append(&_serviceArea);
continue;
}
......@@ -452,7 +448,7 @@ bool WimaController::_fetchContainerData()
if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea = *qobject_cast<const WimaMeasurementAreaData*>(areaData);
areaCounter++;
_visualItems.append(&_measurementArea);
_areas.append(&_measurementArea);
continue;
}
......@@ -468,7 +464,7 @@ bool WimaController::_fetchContainerData()
if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast<const WimaJoinedAreaData*>(areaData);
areaCounter++;
_visualItems.append(&_joinedArea);
_areas.append(&_joinedArea);
continue;
}
......@@ -477,45 +473,29 @@ bool WimaController::_fetchContainerData()
break;
}
// extract mission items
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1)
if (areaCounter != numAreas) {
assert(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;
}
_updateWaypointPath();
for (auto item : tempMissionItems)
_defaultManager.push_back(item.coordinate());
// 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);
_managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
_serviceArea.center().longitude(),
0) );
if(!_calcNextPhase())
if( !_defaultManager.update() ){
assert(false);
return false;
}
// Initialize _scenario.
Area mArea;
......@@ -545,6 +525,7 @@ bool WimaController::_fetchContainerData()
// Check if scenario is defined.
if ( !_verifyScenarioDefinedWithErrorMessage() )
assert(false);
return false;
{
......@@ -585,8 +566,11 @@ bool WimaController::_fetchContainerData()
}
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
......@@ -596,325 +580,102 @@ bool WimaController::_fetchContainerData()
bool WimaController::_calcNextPhase()
{
if (_missionItems.count() < 1) {
_startWaypointIndex = 0;
_endWaypointIndex = 0;
return false;
}
_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
bool value;
_currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if (!reverse) {
if (!getCoordinates(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) {
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");
if ( !_currentManager.next() ) {
assert(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;
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 missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
void WimaController::_updateWaypointPath()
void WimaController::_recalcCurrentPhase()
{
_waypoints.clear();
getCoordinates(_missionItems,
_waypoints,
0,
std::size_t( std::max(_missionItems.count()-1,0) )
);
if ( !_currentManager.update() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateCurrentPath()
void WimaController::_updateOverlap()
{
_currentWaypointPath.clear();
getCoordinates(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1);
bool value;
_currentManager.setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager.update() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateNextWaypoint()
void WimaController::_updateMaxWaypoints()
{
if (_endWaypointIndex < _missionItems.count()-2) {
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_nextPhaseStartWaypointIndex.setRawValue(1 + std::max(_endWaypointIndex + 1 - _overlapWaypoints.rawValue().toInt(), 0));
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
}
}
bool value;
_currentManager.setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
void WimaController::_recalcCurrentPhase()
{
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_nextPhaseStartWaypointIndex.setRawValue(_startWaypointIndex + 1);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
_calcNextPhase();
}
if ( !_currentManager.update() ) {
assert(false);
}
bool WimaController::_setTakeoffLandPosition()
{
_takeoffLandPostion.setAltitude(0);
_takeoffLandPostion.setLongitude(_serviceArea.center().longitude());
_takeoffLandPostion.setLatitude(_serviceArea.center().latitude());
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
void WimaController::_updateflightSpeed()
{
int speedItemCounter = 0;
for (int i = 0; i < _currentMissionItems.count(); i++) {
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());
}
}
}
_managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble());
_currentManager.update();
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
if (speedItemCounter != 3)
qWarning("WimaController::updateflightSpeed(): internal error.");
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateArrivalReturnSpeed()
{
int speedItemCounter = 0;
for (int i = 0; i < _currentMissionItems.count(); i++) {
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.");
_managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble());
_currentManager.update();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateAltitude()
{
for (int i = 0; i < _currentMissionItems.count(); i++) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
qWarning("WimaController::updateAltitude(): nullptr");
return;
}
item->altitude()->setRawValue(_altitude.rawValue().toDouble());
}
_managerSettings.setAltitude(_altitude.rawValue().toDouble());
_currentManager.update();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_checkBatteryLevel()
......@@ -986,18 +747,18 @@ void WimaController::_eventTimerHandler()
void WimaController::_smartRTLCleanUp(bool flying)
{
if ( !flying) { // vehicle has landed
if (_executingSmartRTL) {
_executingSmartRTL = false;
_loadCurrentMissionItemsFromBuffer();
_setPhaseDistance(_phaseDistanceBuffer);
_setPhaseDuration(_phaseDurationBuffer);
_showAllMissionItems.setRawValue(true);
_missionController->removeAllFromVehicle();
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
}
}
// if ( !flying) { // vehicle has landed
// if (_executingSmartRTL) {
// _executingSmartRTL = false;
// _loadCurrentMissionItemsFromBuffer();
// _setPhaseDistance(_phaseDistanceBuffer);
// _setPhaseDuration(_phaseDurationBuffer);
// _showAllMissionItems.setRawValue(true);
// _missionController->removeAllFromVehicle();
// _missionController->removeAll();
// disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
// }
// }
}
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)
{
if (!qFuzzyCompare(distance, _phaseDistance)) {
......@@ -1056,119 +808,6 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
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)
{
if (_vehicleHasLowBattery != batteryLow) {
......@@ -1180,36 +819,36 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow)
void WimaController::_initSmartRTL()
{
QString errorString;
static int attemptCounter = 0;
attemptCounter++;
if (_checkSmartRTLPreCondition(errorString) == true) {
_masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
if (_calcReturnPath(errorString)) {
_executingSmartRTL = true;
attemptCounter = 0;
switch(_srtlReason) {
case BatteryLow:
emit returnBatteryLowConfirmRequired();
break;
case UserRequest:
emit returnUserRequestConfirmRequired();
break;
}
return;
}
}
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."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
}
// QString errorString;
// static int attemptCounter = 0;
// attemptCounter++;
// if (_checkSmartRTLPreCondition(errorString) == true) {
// _masterController->managerVehicle()->pauseVehicle();
// connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
// if (_calcReturnPath(errorString)) {
// _executingSmartRTL = true;
// attemptCounter = 0;
// switch(_srtlReason) {
// case BatteryLow:
// emit returnBatteryLowConfirmRequired();
// break;
// case UserRequest:
// emit returnUserRequestConfirmRequired();
// break;
// }
// return;
// }
// }
// 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."));
// qgcApp()->showMessage(errorString);
// attemptCounter = 0;
// } else {
// _smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
// }
}
void WimaController::_executeSmartRTL()
......@@ -1255,7 +894,8 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage()
void WimaController::_snakeStoreWorkerResults()
{
auto start = std::chrono::high_resolution_clock::now();
_missionItems.clearAndDeleteContents();
_snakeManager.clear();
const WorkerResult_t &r = _snakeWorker.getResult();
_setSnakeCalcInProgress(false);
if (!r.success) {
......@@ -1266,56 +906,20 @@ void WimaController::_snakeStoreWorkerResults()
// create Mission items from r.waypoints
long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
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;
unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first();
QVector<QGeoCoordinate> waypoints;
for (unsigned long i = startIdx; i <= endIdx; ++i) {
QGeoCoordinate wp{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);
_snakeManager.push_back(r.waypoints[int(i)].value<QGeoCoordinate>());
}
_updateWaypointPath();
// 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);
_snakeManager.update();
_calcNextPhase();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
auto end = std::chrono::high_resolution_clock::now();
double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
......@@ -1352,25 +956,6 @@ void WimaController::_initStartSnakeWorker()
_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,
QNemoProgress &progress)
{
......
src/Wima/WimaController.h
View file @ 570fbc03
......@@ -35,6 +35,8 @@
#include "ros_bridge/include/ROSBridge.h"
#include "WaypointManager/DefaultManager.h"
#define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
......@@ -136,10 +138,6 @@ public:
READ arrivalReturnSpeed
CONSTANT
)
Q_PROPERTY(Fact* reverse
READ reverse
CONSTANT
)
Q_PROPERTY(bool uploadOverrideRequired
READ uploadOverrideRequired
WRITE setUploadOverrideRequired
......@@ -216,10 +214,10 @@ public:
// QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon joinedArea (void) const;
WimaDataContainer* dataContainer (void) { return _container; }
QmlObjectListModel* missionItems (void) { return &_missionItems; }
QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; }
QVariantList waypointPath (void) const;
QVariantList currentWaypointPath (void) { return _currentWaypointPath; }
QmlObjectListModel* missionItems (void);
QmlObjectListModel* currentMissionItems (void);
QVariantList waypointPath (void);
QVariantList currentWaypointPath (void);
Fact* enableWimaController (void) { return &_enableWimaController; }
Fact* overlapWaypoints (void) { return &_overlapWaypoints; }
Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; }
......@@ -229,7 +227,6 @@ public:
Fact* flightSpeed (void) { return &_flightSpeed; }
Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; }
Fact* altitude (void) { return &_altitude; }
Fact* reverse (void) { return &_reverse; }
Fact* enableSnake (void) { return &_enableSnake; }
Fact* snakeTileWidth (void) { return &_snakeTileWidth;}
......@@ -291,7 +288,6 @@ public:
static const char* flightSpeedName;
static const char* arrivalReturnSpeedName;
static const char* altitudeName;
static const char* reverseName;
static const char* snakeTileWidthName;
static const char* snakeTileHeightName;
static const char* snakeMinTileAreaName;
......@@ -330,11 +326,13 @@ private:
private slots:
bool _fetchContainerData();
bool _calcNextPhase(void);
void _updateWaypointPath (void);
void _updateCurrentPath (void);
void _updateNextWaypoint (void);
//void _updateWaypointPath (void);
//void _updateCurrentPath (void);
//void _updateNextWaypoint (void);
void _recalcCurrentPhase (void);
bool _setTakeoffLandPosition (void);
//bool _setTakeoffLandPosition (void);
void _updateOverlap (void);
void _updateMaxWaypoints (void);
void _updateflightSpeed (void);
void _updateArrivalReturnSpeed (void);
void _updateAltitude (void);
......@@ -342,7 +340,6 @@ private slots:
void _eventTimerHandler (void);
void _smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL
void _enableDisableLowBatteryHandling (QVariant enable);
void _reverseChangedHandler ();
void _initSmartRTL ();
void _executeSmartRTL ();
void _setSnakeConnectionStatus (SnakeConnectionStatus status);
......@@ -369,19 +366,27 @@ private:
MissionController *_missionController;
// QString _currentFile; // file for saveing
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
WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea
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
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;
WaypointManager::AreaInterface _areaInterface;
WaypointManager::Settings _managerSettings;
WaypointManager::DefaultManager _defaultManager;
WaypointManager::DefaultManager _snakeManager;
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;
......@@ -395,13 +400,12 @@ private:
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _arrivalReturnSpeed; // arrival and return path speed
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)
int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element
// (which is not part of the return path) of _currentMissionItem
int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element
// (which is not part of the arrival path) of _currentMissionItem
// int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element
// // (which is not part of the return path) of _currentMissionItem
// int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element
// // (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.
// 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
......
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