Skip to content

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

wima controller mod, appimage updated

parent 4acc1d5d
Show whitespace changes
Inline Side-by-side
Showing with 752 additions and 544 deletions
qgroundcontrol.pro
View file @ c65f8eca
......@@ -448,7 +448,6 @@ HEADERS += \
src/Wima/Snake/QNemoHeartbeat.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/QNemoProgress.h \
src/Wima/Snake/SnakeThread.h \
src/Wima/Snake/SnakeTile.h \
src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \
......@@ -513,7 +512,6 @@ SOURCES += \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeThread.cc \
src/Wima/Snake/SnakeTile.cpp \
src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ c65f8eca
......@@ -241,43 +241,66 @@ FlightMap {
color: "green"
}
// Add Snake tile center points to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
model: _enable ? wimaController.snakeTileCenterPoints : 0
delegate: ProgressIndicator{
coordinate: modelData
currentValue: getProgress()
width: 10
height: 10
z: 1
function getProgress() {
var progress = wimaController.nemoProgress[index]
if (progress < 0)
progress = 0
if (progress > 100)
progress = 100
return progress
}
}
}
// // Add Snake tile center points to the map
// MapItemView {
// id:progressView
// property bool _enable: wimaController.enableWimaController.value
// && wimaController.enableSnake.value
// property bool valid: wimaController.snakeTileCenterPoints.length
// === wimaController.nemoProgress.length
// model: _enable ? wimaController.snakeTileCenterPoints : 0
// delegate: ProgressIndicator{
// coordinate: modelData
// currentValue: getProgress()
// z: 1
// function getProgress() {
// var progress = 0
// if (progressView.valid){
// progress = wimaController.nemoProgress[index]
// }
// if (progress < 0)
// progress = 0
// if (progress > 100)
// progress = 100
// return progress
// }
// }
// }
// Add Snake tiles to the map
MapItemView {
id: tileView
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
property bool valid: wimaController.snakeTileCenterPoints.length
=== wimaController.nemoProgress.length
model: _enable ? wimaController.snakeTiles : 0
delegate: MapPolygon{
path: object.path;
border.color: "black"
border.width: 1
color: "transparent"
opacity: 1
color: getColor()
opacity: 0.6
z: 2
function getColor() {
var progress = 0
if (tileView.valid){
progress = wimaController.nemoProgress[index]
}
if (progress < 25)
return "transparent"
if (progress < 50)
return "orange"
if (progress < 75)
return "yellow"
if (progress < 100)
return "greenyellow"
return "limegreen"
}
}
}
......
src/MissionManager/MissionItem.cc
View file @ c65f8eca
......@@ -7,83 +7,71 @@
*
****************************************************************************/
#include <QStringList>
#include <QDebug>
#include <QStringList>
#include "MissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
#include "MissionItem.h"
#include "QGCApplication.h"
#include "VisualMissionItem.h"
const char* MissionItem::_jsonFrameKey = "frame";
const char* MissionItem::_jsonCommandKey = "command";
const char* MissionItem::_jsonAutoContinueKey = "autoContinue";
const char* MissionItem::_jsonCoordinateKey = "coordinate";
const char* MissionItem::_jsonParamsKey = "params";
const char* MissionItem::_jsonDoJumpIdKey = "doJumpId";
const char *MissionItem::_jsonFrameKey = "frame";
const char *MissionItem::_jsonCommandKey = "command";
const char *MissionItem::_jsonAutoContinueKey = "autoContinue";
const char *MissionItem::_jsonCoordinateKey = "coordinate";
const char *MissionItem::_jsonParamsKey = "params";
const char *MissionItem::_jsonDoJumpIdKey = "doJumpId";
// Deprecated V1 format keys
const char* MissionItem::_jsonParam1Key = "param1";
const char* MissionItem::_jsonParam2Key = "param2";
const char* MissionItem::_jsonParam3Key = "param3";
const char* MissionItem::_jsonParam4Key = "param4";
MissionItem::MissionItem(QObject* parent)
: QObject(parent)
, _sequenceNumber(0)
, _doJumpId(-1)
, _isCurrentItem(false)
, _autoContinueFact (0, "AutoContinue", FactMetaData::valueTypeUint32)
, _commandFact (0, "", FactMetaData::valueTypeUint32)
, _frameFact (0, "", FactMetaData::valueTypeUint32)
, _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble)
, _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble)
, _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble)
, _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble)
, _param5Fact (0, "Latitude:", FactMetaData::valueTypeDouble)
, _param6Fact (0, "Longitude:", FactMetaData::valueTypeDouble)
, _param7Fact (0, "Altitude:", FactMetaData::valueTypeDouble)
{
const char *MissionItem::_jsonParam1Key = "param1";
const char *MissionItem::_jsonParam2Key = "param2";
const char *MissionItem::_jsonParam3Key = "param3";
const char *MissionItem::_jsonParam4Key = "param4";
MissionItem::MissionItem(QObject *parent)
: QObject(parent),
_autoContinueFact(0, "AutoContinue", FactMetaData::valueTypeUint32),
_commandFact(0, "", FactMetaData::valueTypeUint32),
_frameFact(0, "", FactMetaData::valueTypeUint32),
_param1Fact(0, "Param1:", FactMetaData::valueTypeDouble),
_param2Fact(0, "Param2:", FactMetaData::valueTypeDouble),
_param3Fact(0, "Param3:", FactMetaData::valueTypeDouble),
_param4Fact(0, "Param4:", FactMetaData::valueTypeDouble),
_param5Fact(0, "Latitude:", FactMetaData::valueTypeDouble),
_param6Fact(0, "Longitude:", FactMetaData::valueTypeDouble),
_param7Fact(0, "Altitude:", FactMetaData::valueTypeDouble),
_sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) {
// Need a good command and frame before we start passing signals around
_commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
_frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
setAutoContinue(true);
connect(&_param1Fact, &Fact::rawValueChanged, this, &MissionItem::_param1Changed);
connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
connect(&_param1Fact, &Fact::rawValueChanged, this,
&MissionItem::_param1Changed);
connect(&_param2Fact, &Fact::rawValueChanged, this,
&MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this,
&MissionItem::_param3Changed);
}
MissionItem::MissionItem(int sequenceNumber,
MAV_CMD command,
MAV_FRAME frame,
double param1,
double param2,
double param3,
double param4,
double param5,
double param6,
double param7,
bool autoContinue,
bool isCurrentItem,
QObject* parent)
: QObject(parent)
, _sequenceNumber(sequenceNumber)
, _doJumpId(-1)
, _isCurrentItem(isCurrentItem)
, _commandFact (0, "", FactMetaData::valueTypeUint32)
, _frameFact (0, "", FactMetaData::valueTypeUint32)
, _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble)
, _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble)
, _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble)
, _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble)
, _param5Fact (0, "Lat/X:", FactMetaData::valueTypeDouble)
, _param6Fact (0, "Lon/Y:", FactMetaData::valueTypeDouble)
, _param7Fact (0, "Alt/Z:", FactMetaData::valueTypeDouble)
{
MissionItem::MissionItem(int sequenceNumber, MAV_CMD command, MAV_FRAME frame,
double param1, double param2, double param3,
double param4, double param5, double param6,
double param7, bool autoContinue, bool isCurrentItem,
QObject *parent)
: QObject(parent), _commandFact(0, "", FactMetaData::valueTypeUint32),
_frameFact(0, "", FactMetaData::valueTypeUint32),
_param1Fact(0, "Param1:", FactMetaData::valueTypeDouble),
_param2Fact(0, "Param2:", FactMetaData::valueTypeDouble),
_param3Fact(0, "Param3:", FactMetaData::valueTypeDouble),
_param4Fact(0, "Param4:", FactMetaData::valueTypeDouble),
_param5Fact(0, "Lat/X:", FactMetaData::valueTypeDouble),
_param6Fact(0, "Lon/Y:", FactMetaData::valueTypeDouble),
_param7Fact(0, "Alt/Z:", FactMetaData::valueTypeDouble),
_sequenceNumber(sequenceNumber), _doJumpId(-1),
_isCurrentItem(isCurrentItem) {
// Need a good command and frame before we start passing signals around
_commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
_frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
......@@ -100,37 +88,36 @@ MissionItem::MissionItem(int sequenceNumber,
_param6Fact.setRawValue(param6);
_param7Fact.setRawValue(param7);
connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
connect(&_param2Fact, &Fact::rawValueChanged, this,
&MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this,
&MissionItem::_param3Changed);
}
MissionItem::MissionItem(const MissionItem& other, QObject* parent)
: QObject(parent)
, _sequenceNumber(0)
, _doJumpId(-1)
, _isCurrentItem(false)
, _commandFact (0, "", FactMetaData::valueTypeUint32)
, _frameFact (0, "", FactMetaData::valueTypeUint32)
, _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble)
, _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble)
, _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble)
, _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble)
, _param5Fact (0, "Lat/X:", FactMetaData::valueTypeDouble)
, _param6Fact (0, "Lon/Y:", FactMetaData::valueTypeDouble)
, _param7Fact (0, "Alt/Z:", FactMetaData::valueTypeDouble)
{
MissionItem::MissionItem(const MissionItem &other, QObject *parent)
: QObject(parent), _commandFact(0, "", FactMetaData::valueTypeUint32),
_frameFact(0, "", FactMetaData::valueTypeUint32),
_param1Fact(0, "Param1:", FactMetaData::valueTypeDouble),
_param2Fact(0, "Param2:", FactMetaData::valueTypeDouble),
_param3Fact(0, "Param3:", FactMetaData::valueTypeDouble),
_param4Fact(0, "Param4:", FactMetaData::valueTypeDouble),
_param5Fact(0, "Lat/X:", FactMetaData::valueTypeDouble),
_param6Fact(0, "Lon/Y:", FactMetaData::valueTypeDouble),
_param7Fact(0, "Alt/Z:", FactMetaData::valueTypeDouble),
_sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) {
// Need a good command and frame before we start passing signals around
_commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
_frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
*this = other;
connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
connect(&_param2Fact, &Fact::rawValueChanged, this,
&MissionItem::_param2Changed);
connect(&_param3Fact, &Fact::rawValueChanged, this,
&MissionItem::_param3Changed);
}
const MissionItem& MissionItem::operator=(const MissionItem& other)
{
const MissionItem &MissionItem::operator=(const MissionItem &other) {
_doJumpId = other._doJumpId;
setCommand(other.command());
......@@ -150,28 +137,28 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
return *this;
}
MissionItem::~MissionItem()
{
MissionItem::~MissionItem() {}
}
void MissionItem::save(QJsonObject& json) const
{
json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue;
void MissionItem::save(QJsonObject &json) const {
json[VisualMissionItem::jsonTypeKey] =
VisualMissionItem::jsonTypeSimpleItemValue;
json[_jsonFrameKey] = frame();
json[_jsonCommandKey] = command();
json[_jsonAutoContinueKey] = autoContinue();
json[_jsonDoJumpIdKey] = _sequenceNumber;
QJsonArray rgParams = { param1(), param2(), param3(), param4(), param5(), param6(), param7() };
QJsonArray rgParams = {param1(), param2(), param3(), param4(),
param5(), param6(), param7()};
json[_jsonParamsKey] = rgParams;
}
bool MissionItem::load(QTextStream &loadStream)
{
bool MissionItem::load(QTextStream &loadStream) {
const QStringList &wpParams = loadStream.readLine().split("\t");
if (wpParams.size() == 12) {
setCommand((MAV_CMD)wpParams[3].toInt()); // Has to be first since it triggers defaults to be set, which are then override by below set calls
setCommand(
(MAV_CMD)wpParams[3]
.toInt()); // Has to be first since it triggers defaults to be set,
// which are then override by below set calls
setSequenceNumber(wpParams[0].toInt());
setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
setFrame((MAV_FRAME)wpParams[2].toInt());
......@@ -189,8 +176,9 @@ bool MissionItem::load(QTextStream &loadStream)
return false;
}
bool MissionItem::_convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Json, QString& errorString)
{
bool MissionItem::_convertJsonV1ToV2(const QJsonObject &json,
QJsonObject &v2Json,
QString &errorString) {
// V1 format type = "missionItem", V2 format type = "MissionItem"
// V1 format has params in separate param[1-n] keys
// V2 format has params in params array
......@@ -202,21 +190,25 @@ bool MissionItem::_convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Jso
}
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ _jsonParam1Key, QJsonValue::Double, true },
{ _jsonParam2Key, QJsonValue::Double, true },
{ _jsonParam3Key, QJsonValue::Double, true },
{ _jsonParam4Key, QJsonValue::Double, true },
{VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
{_jsonParam1Key, QJsonValue::Double, true},
{_jsonParam2Key, QJsonValue::Double, true},
{_jsonParam3Key, QJsonValue::Double, true},
{_jsonParam4Key, QJsonValue::Double, true},
};
if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
return false;
}
if (v2Json[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("missionItem")) {
v2Json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue;
if (v2Json[VisualMissionItem::jsonTypeKey].toString() ==
QStringLiteral("missionItem")) {
v2Json[VisualMissionItem::jsonTypeKey] =
VisualMissionItem::jsonTypeSimpleItemValue;
}
QJsonArray rgParams = { json[_jsonParam1Key].toDouble(), json[_jsonParam2Key].toDouble(), json[_jsonParam3Key].toDouble(), json[_jsonParam4Key].toDouble() };
QJsonArray rgParams = {
json[_jsonParam1Key].toDouble(), json[_jsonParam2Key].toDouble(),
json[_jsonParam3Key].toDouble(), json[_jsonParam4Key].toDouble()};
v2Json[_jsonParamsKey] = rgParams;
v2Json.remove(_jsonParam1Key);
v2Json.remove(_jsonParam2Key);
......@@ -226,8 +218,7 @@ bool MissionItem::_convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Jso
return true;
}
bool MissionItem::_convertJsonV2ToV3(QJsonObject& json, QString& errorString)
{
bool MissionItem::_convertJsonV2ToV3(QJsonObject &json, QString &errorString) {
// V2 format: param 5/6/7 stored in GeoCoordinate
// V3 format: param 5/6/7 stored in params array
......@@ -237,14 +228,16 @@ bool MissionItem::_convertJsonV2ToV3(QJsonObject& json, QString& errorString)
}
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ _jsonCoordinateKey, QJsonValue::Array, true },
{_jsonCoordinateKey, QJsonValue::Array, true},
};
if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
return false;
}
QGeoCoordinate coordinate;
if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey],
true /* altitudeRequired */, coordinate,
errorString)) {
return false;
}
......@@ -259,8 +252,8 @@ bool MissionItem::_convertJsonV2ToV3(QJsonObject& json, QString& errorString)
return true;
}
bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString)
{
bool MissionItem::load(const QJsonObject &json, int sequenceNumber,
QString &errorString) {
QJsonObject convertedJson;
if (!_convertJsonV1ToV2(json, convertedJson, errorString)) {
return false;
......@@ -270,19 +263,23 @@ bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& err
}
QList<JsonHelper::KeyValidateInfo> keyInfoList = {
{ VisualMissionItem::jsonTypeKey, QJsonValue::String, true },
{ _jsonFrameKey, QJsonValue::Double, true },
{ _jsonCommandKey, QJsonValue::Double, true },
{ _jsonParamsKey, QJsonValue::Array, true },
{ _jsonAutoContinueKey, QJsonValue::Bool, true },
{ _jsonDoJumpIdKey, QJsonValue::Double, false },
{VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
{_jsonFrameKey, QJsonValue::Double, true},
{_jsonCommandKey, QJsonValue::Double, true},
{_jsonParamsKey, QJsonValue::Array, true},
{_jsonAutoContinueKey, QJsonValue::Bool, true},
{_jsonDoJumpIdKey, QJsonValue::Double, false},
};
if (!JsonHelper::validateKeys(convertedJson, keyInfoList, errorString)) {
return false;
}
if (convertedJson[VisualMissionItem::jsonTypeKey] != VisualMissionItem::jsonTypeSimpleItemValue) {
errorString = tr("Type found: %1 must be: %2").arg(convertedJson[VisualMissionItem::jsonTypeKey].toString()).arg(VisualMissionItem::jsonTypeSimpleItemValue);
if (convertedJson[VisualMissionItem::jsonTypeKey] !=
VisualMissionItem::jsonTypeSimpleItemValue) {
errorString =
tr("Type found: %1 must be: %2")
.arg(convertedJson[VisualMissionItem::jsonTypeKey].toString())
.arg(VisualMissionItem::jsonTypeSimpleItemValue);
return false;
}
......@@ -292,9 +289,12 @@ bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& err
return false;
}
for (int i=0; i<4; i++) {
if (rgParams[i].type() != QJsonValue::Double && rgParams[i].type() != QJsonValue::Null) {
errorString = tr("Param %1 incorrect type %2, must be double or null").arg(i+1).arg(rgParams[i].type());
for (int i = 0; i < 4; i++) {
if (rgParams[i].type() != QJsonValue::Double &&
rgParams[i].type() != QJsonValue::Null) {
errorString = tr("Param %1 incorrect type %2, must be double or null")
.arg(i + 1)
.arg(rgParams[i].type());
return false;
}
}
......@@ -322,137 +322,123 @@ bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& err
return true;
}
void MissionItem::setSequenceNumber(int sequenceNumber)
{
void MissionItem::setSequenceNumber(int sequenceNumber) {
if (_sequenceNumber != sequenceNumber) {
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(_sequenceNumber);
}
}
void MissionItem::setCommand(MAV_CMD command)
{
void MissionItem::setCommand(MAV_CMD command) {
if ((MAV_CMD)this->command() != command) {
_commandFact.setRawValue(command);
}
}
void MissionItem::setFrame(MAV_FRAME frame)
{
void MissionItem::setFrame(MAV_FRAME frame) {
if (this->frame() != frame) {
_frameFact.setRawValue(frame);
}
}
void MissionItem::setAutoContinue(bool autoContinue)
{
void MissionItem::setAutoContinue(bool autoContinue) {
if (this->autoContinue() != autoContinue) {
_autoContinueFact.setRawValue(autoContinue);
}
}
void MissionItem::setIsCurrentItem(bool isCurrentItem)
{
void MissionItem::setIsCurrentItem(bool isCurrentItem) {
if (_isCurrentItem != isCurrentItem) {
_isCurrentItem = isCurrentItem;
emit isCurrentItemChanged(isCurrentItem);
}
}
void MissionItem::setParam1(double param)
{
void MissionItem::setParam1(double param) {
if (param1() != param) {
_param1Fact.setRawValue(param);
}
}
void MissionItem::setParam2(double param)
{
void MissionItem::setParam2(double param) {
if (param2() != param) {
_param2Fact.setRawValue(param);
}
}
void MissionItem::setParam3(double param)
{
void MissionItem::setParam3(double param) {
if (param3() != param) {
_param3Fact.setRawValue(param);
}
}
void MissionItem::setParam4(double param)
{
void MissionItem::setParam4(double param) {
if (param4() != param) {
_param4Fact.setRawValue(param);
}
}
void MissionItem::setParam5(double param)
{
void MissionItem::setParam5(double param) {
if (param5() != param) {
_param5Fact.setRawValue(param);
}
}
void MissionItem::setParam6(double param)
{
void MissionItem::setParam6(double param) {
if (param6() != param) {
_param6Fact.setRawValue(param);
}
}
void MissionItem::setParam7(double param)
{
void MissionItem::setParam7(double param) {
if (param7() != param) {
_param7Fact.setRawValue(param);
}
}
QGeoCoordinate MissionItem::coordinate(void) const
{
if(!std::isfinite(param5()) || !std::isfinite(param6())) {
//-- If either of these are NAN, return an invalid (QGeoCoordinate::isValid() == false) coordinate
QGeoCoordinate MissionItem::coordinate(void) const {
if (!std::isfinite(param5()) || !std::isfinite(param6())) {
//-- If either of these are NAN, return an invalid
//(QGeoCoordinate::isValid() == false) coordinate
return QGeoCoordinate();
}
return QGeoCoordinate(param5(), param6(), param7());
}
double MissionItem::specifiedFlightSpeed(void) const
{
double MissionItem::specifiedFlightSpeed(void) const {
double flightSpeed = std::numeric_limits<double>::quiet_NaN();
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_CHANGE_SPEED && _param2Fact.rawValue().toDouble() > 0) {
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_CHANGE_SPEED &&
_param2Fact.rawValue().toDouble() > 0) {
flightSpeed = _param2Fact.rawValue().toDouble();
}
return flightSpeed;
}
double MissionItem::specifiedGimbalYaw(void) const
{
double MissionItem::specifiedGimbalYaw(void) const {
double gimbalYaw = std::numeric_limits<double>::quiet_NaN();
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) {
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL &&
_param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) {
gimbalYaw = _param3Fact.rawValue().toDouble();
}
return gimbalYaw;
}
double MissionItem::specifiedGimbalPitch(void) const
{
double MissionItem::specifiedGimbalPitch(void) const {
double gimbalPitch = std::numeric_limits<double>::quiet_NaN();
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) {
if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL &&
_param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) {
gimbalPitch = _param1Fact.rawValue().toDouble();
}
return gimbalPitch;
}
void MissionItem::_param1Changed(QVariant value)
{
void MissionItem::_param1Changed(QVariant value) {
Q_UNUSED(value);
double gimbalPitch = specifiedGimbalPitch();
......@@ -461,8 +447,7 @@ void MissionItem::_param1Changed(QVariant value)
}
}
void MissionItem::_param2Changed(QVariant value)
{
void MissionItem::_param2Changed(QVariant value) {
Q_UNUSED(value);
double flightSpeed = specifiedFlightSpeed();
......@@ -471,8 +456,7 @@ void MissionItem::_param2Changed(QVariant value)
}
}
void MissionItem::_param3Changed(QVariant value)
{
void MissionItem::_param3Changed(QVariant value) {
Q_UNUSED(value);
double gimbalYaw = specifiedGimbalYaw();
......
src/QmlControls/QmlObjectListModel.cc
View file @ c65f8eca
......@@ -121,9 +121,9 @@ const QObject *QmlObjectListModel::operator[](int index) const {
return _objectList[index];
}
bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
bool QmlObjectListModel::operator==(const QmlObjectListModel &other) const {
if (this->count() == other.count()) {
for (std::size_t i = 0; i < this->count(); ++i) {
for (int i = 0; i < this->count(); ++i) {
if (this->get(i) != other.get(i)) {
return false;
}
......@@ -134,7 +134,7 @@ bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
}
}
bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
bool QmlObjectListModel::operator!=(const QmlObjectListModel &other) const {
return !this->operator==(other);
}
......
src/QmlControls/QmlObjectListModel.h
View file @ c65f8eca
......@@ -51,8 +51,8 @@ public:
}
QList<QObject *> *objectList() { return &_objectList; }
bool operator==(const QmlObjectListModel &other);
bool operator!=(const QmlObjectListModel &other);
bool operator==(const QmlObjectListModel &other) const;
bool operator!=(const QmlObjectListModel &other) const;
/// Calls deleteLater on all items and this itself.
void deleteListAndContents();
......
src/Wima/CircularSurvey.cc
View file @ c65f8eca
......@@ -359,7 +359,12 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
// Store raw transects.
const auto &transectsENU = this->_workerOutput->transects;
const auto &ori = this->_referencePoint;
for (auto &t : transectsENU) {
std::size_t startIdx = 0;
if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
startIdx = 1;
}
for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
const auto &t = transectsENU[i];
QList<QGeoCoordinate> trGeo;
for (auto &v : t) {
QGeoCoordinate c;
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ c65f8eca
......@@ -28,6 +28,15 @@ TileData &TileData::operator=(const TileData &other) {
return *this;
}
bool TileData::operator==(const TileData &other) const {
return this->tiles == other.tiles &&
this->tileCenterPoints == other.tileCenterPoints;
}
bool TileData::operator!=(const TileData &other) const {
return !this->operator==(other);
}
void TileData::clear() {
this->tiles.clearAndDeleteContents();
this->tileCenterPoints.clear();
......
src/Wima/Geometry/WimaMeasurementArea.h
View file @ c65f8eca
......@@ -17,6 +17,8 @@ public:
~TileData();
TileData &operator=(const TileData &other);
bool operator==(const TileData &other) const;
bool operator!=(const TileData &other) const;
void clear();
std::size_t size() const;
......
src/Wima/Geometry/WimaMeasurementAreaData.cc
View file @ c65f8eca
......@@ -61,10 +61,10 @@ QVariantList &WimaMeasurementAreaData::tileCenterPoints() {
}
const TileData &WimaMeasurementAreaData::tileData() const {
return this->tileData();
return this->_tileData;
}
TileData &WimaMeasurementAreaData::tileData() { return this->tileData(); }
TileData &WimaMeasurementAreaData::tileData() { return this->_tileData; }
const QVector<int> &WimaMeasurementAreaData::progress() const {
return this->_progress;
......
src/Wima/RoutingThread.cpp
View file @ c65f8eca
......@@ -21,7 +21,7 @@ RoutingThread::~RoutingThread() {
this->wait();
}
bool RoutingThread::calculating() { return this->_calculating; }
bool RoutingThread::calculating() const { return this->_calculating; }
void RoutingThread::route(const snake::BoostPolygon &safeArea,
const RoutingThread::Generator &generator) {
......@@ -107,7 +107,7 @@ void RoutingThread::run() {
} // end calculation
#ifdef DEBUG_CIRCULAR_SURVEY
else {
qWarning() << "RoutingWorker::run(): preconditions failed.";
qWarning() << "RoutingWorker::run(): generator() failed.";
}
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
......
src/Wima/RoutingThread.h
View file @ c65f8eca
......@@ -27,11 +27,12 @@ class RoutingThread : public QThread {
public:
using PtrRoutingData = QSharedPointer<RoutingData>;
using Generator = std::function<bool(snake::Transects &)>;
using Consumer = std::function<void(const RoutingData &)>;
RoutingThread(QObject *parent = nullptr);
~RoutingThread() override;
bool calculating();
bool calculating() const;
public slots:
void route(const snake::BoostPolygon &safeArea, const Generator &generator);
......
src/Wima/Snake/NemoInterface.cpp
View file @ c65f8eca
#include "NemoInterface.h"
#include "SnakeTilesLocal.h"
#include "QGCApplication.h"
#include "QGCToolbox.h"
......@@ -135,12 +136,12 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
bool error = false;
for (std::size_t i = 0; i < tileData.tiles.count(); ++i) {
*obj = tileData.tiles.get(i);
*tile = qobject_cast<const SnakeTile *>(obj);
for (int i = 0; i < tileData.tiles.count(); ++i) {
obj = tileData.tiles.get(i);
tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::BoostPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
SnakeTileLocal tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
......@@ -162,7 +163,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
}
bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
return this->tileData = tileData;
return this->tileData == tileData;
}
NemoInterface::NemoStatus NemoInterface::Impl::status() {
......
src/Wima/Snake/NemoInterface.h
View file @ c65f8eca
......@@ -3,8 +3,6 @@
#include <QGeoCoordinate>
#include <QObject>
#include "SnakeTilesLocal.h"
#include <memory>
class TileData;
......
src/Wima/Snake/SnakeTileLocal.h
View file @ c65f8eca
#pragma once
#include "Wima/Geometry/GenericPolygon.h"
using SnakeTileLocal = GenericPolygon<>;
using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;
src/Wima/Snake/SnakeTilesLocal.h
View file @ c65f8eca
#pragma once
#include "Wima/Snake/SnakeTileLocal.h"
#include "Wima/Geometry/GenericPolygonArray.h"
typedef GenericPolygonArray<SnakeTileLocal, QVector> SnakeTilesLocal;
#include "Wima/Snake/SnakeTileLocal.h"
#include <vector>
typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;
src/Wima/Snake/snake.h
View file @ c65f8eca
......@@ -92,9 +92,9 @@ struct BoundingBox {
BoostPolygon corners;
};
template <class GeoPoint>
void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, BoostPoint &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
......@@ -108,9 +108,25 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
(void)z;
}
template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
double x = 0, y = 0, z = 0;
auto alt = in.altitude();
alt = std::isnan(alt) ? 0 : alt;
proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
out.setX(x);
out.setY(y);
(void)z;
}
template <class GeoPoint>
void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
GeographicLib::Constants::WGS84_f());
GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
origin.altitude(), earth);
......@@ -122,10 +138,18 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
out.setAltitude(alt);
}
template <class GeoPoint, template <class, class...> class Container>
void areaToEnu(const GeoPoint &origin, const Container<GeoPoint> &in,
BoostPolygon &out) {
for (auto vertex : in) {
template <class GeoPoint, class Container1, class Container2>
void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
for (auto &vertex : in) {
typename Container2::value_type p;
toENU(origin, vertex, p);
out.push_back(p);
}
}
template <class GeoPoint, class Container>
void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) {
for (auto &vertex : in) {
BoostPoint p;
toENU(origin, vertex, p);
out.outer().push_back(p);
......@@ -133,11 +157,21 @@ void areaToEnu(const GeoPoint &origin, const Container<GeoPoint> &in,
bg::correct(out);
}
template <class GeoPoint, template <class, class...> class Container>
template <class GeoPoint, class Container1, class Container2>
void areaFromEnu(const GeoPoint &origin, Container1 &in,
const Container2 &out) {
for (auto &vertex : in) {
typename Container2::value_type p;
fromENU(origin, vertex, p);
out.push_back(p);
}
}
template <class GeoPoint, class Container>
void areaFromEnu(const GeoPoint &origin, BoostPolygon &in,
const Container<GeoPoint> &out) {
for (auto vertex : in.outer()) {
GeoPoint p;
const Container &out) {
for (auto &vertex : in.outer()) {
typename Container::value_type p;
fromENU(origin, vertex, p);
out.push_back(p);
}
......
src/Wima/WimaController.cc
View file @ c65f8eca
......@@ -6,14 +6,20 @@
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "SimpleMissionItem.h"
#include "WimaSettings.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoProgress.h"
#include "Snake/SnakeTile.h"
#include "QVector3D"
#include <QScopedPointer>
#include <QtConcurrentRun>
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"
#include <memory>
......@@ -48,7 +54,7 @@ WimaController::StatusMap WimaController::_nemoStatusMap{
WimaController::WimaController(QObject *parent)
: QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(),
_corridor(), _localPlanDataValid(false),
_corridor(), _planDataValid(false),
_areaInterface(&_measurementArea, &_serviceArea, &_corridor,
&_joinedArea),
_WMSettings(), _defaultWM(_WMSettings, _areaInterface),
......@@ -141,6 +147,12 @@ WimaController::WimaController(QObject *parent)
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_switchToSnakeWaypointManager);
_switchToSnakeWaypointManager(_enableSnake.rawValue());
// Routing
connect(&_routingThread, &RoutingThread::result, this,
&WimaController::_storeRoute);
connect(&_routingThread, &RoutingThread::calculatingChanged, this,
&WimaController::snakeCalcInProgressChanged);
}
PlanMasterController *WimaController::masterController() {
......@@ -192,7 +204,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() {
return &this->_measurementArea.tiles();
return this->_measurementArea.tiles();
}
QVariantList WimaController::snakeTileCenterPoints() {
......@@ -222,8 +234,7 @@ QString WimaController::nemoStatusString() const {
}
bool WimaController::snakeCalcInProgress() const {
qWarning() << "using snakeCalcInProgress dummy";
return true;
return _routingThread.calculating();
}
void WimaController::setMasterController(PlanMasterController *masterC) {
......@@ -393,7 +404,7 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
emit snakeTilesChanged();
emit nemoProgressChanged();
_localPlanDataValid = false;
_planDataValid = false;
// extract list with WimaAreas
QList<const WimaAreaData *> areaList = planData->areaList();
......@@ -452,20 +463,6 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
emit visualItemsChanged();
emit snakeTilesChanged();
// Publish tiles if necessary.
if (!this->_nemoInterface.hasTileData(this->_measurementArea.tileData())) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
this->_measurementArea.progress() =
QVector<int>(this->_measurementArea.tiles()->count, 0);
emit nemoProgressChanged();
} else if (this->_enableSnake.rawValue().toBool()) {
const auto progess = this->_nemoInterface.progress();
if (progess.size() == this->_measurementArea.tiles()->count()) {
this->_measurementArea.progress() = std::move(progress);
emit nemoProgressChanged();
}
}
// Copy raw transects.
this->_rawTransects = planData->transects();
......@@ -477,9 +474,9 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
}
for (auto *item : tempMissionItems) {
_snakeWM.push_back(item->coordinate());
_defaultWM.push_back(item->coordinate());
}
// // extract mission items
_WMSettings.setHomePosition(QGeoCoordinate(
_serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0));
// auto tempMissionItems = planData->missionItems();
......@@ -487,15 +484,37 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
Q_ASSERT(false);
return false;
}
// if (tempMissionItems.size() < 1) {
if (!_snakeWM.reset()) {
Q_ASSERT(false);
return false;
}
if (_currentWM == &_defaultWM || _currentWM == &_snakeWM) {
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
}
// Publish tiles, set progress if necessary.
if (!this->_nemoInterface.hasTileData(this->_measurementArea.tileData())) {
if (_enableSnake.rawValue().toBool()) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
}
this->_measurementArea.progress() =
QVector<int>(this->_measurementArea.tiles()->count(), 0);
emit nemoProgressChanged();
qWarning("WimaController: add snake update here lkjdfl!");
} else if (this->_enableSnake.rawValue().toBool()) {
const auto progress = this->_nemoInterface.progress();
if (progress.size() == this->_measurementArea.tiles()->count()) {
this->_measurementArea.progress() = std::move(progress);
emit nemoProgressChanged();
this->_updateRoute();
}
}
_localPlanDataValid = true;
_planDataValid = true;
return true;
}
......@@ -701,7 +720,7 @@ void WimaController::_setPhaseDuration(double duration) {
}
bool WimaController::_SRTLPrecondition(QString &errorString) {
if (!_localPlanDataValid) {
if (!_planDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a "
"measurement and a service area."));
return false;
......@@ -772,20 +791,53 @@ void WimaController::_initSmartRTL() {
}
}
void WimaController::_threadFinishedHandler() {
qWarning() << "update here as well .jisddf";
void WimaController::_storeRoute(RoutingThread::PtrRoutingData data) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
// Copy waypoints to waypoint manager.
_snakeWM.clear();
auto waypoints = 1;
if (waypoints.size() < 1) {
return;
if (data->route.size() > 0) {
// Store route.
const auto &transectsENU = data->transects;
const auto &transectsInfo = data->transectsInfo;
const auto &route = data->route;
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst = transectsInfo.front();
const auto &firstTransect = transectsENU[infoFirst.index];
const auto &firstWaypoint =
infoFirst.reversed ? firstTransect.back() : firstTransect.front();
double th = 0.001;
for (std::size_t i = 0; i < route.size(); ++i) {
auto dist = bg::distance(route[i], firstWaypoint);
if (dist < th) {
idxFirst = i;
break;
}
for (auto &vertex : waypoints) {
_snakeWM.push_back(vertex);
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.back();
const auto &lastTransect = transectsENU[infoLast.index];
const auto &lastWaypoint =
infoLast.reversed ? lastTransect.front() : lastTransect.back();
for (long i = route.size() - 1; i >= 0; --i) {
auto dist = bg::distance(route[i], lastWaypoint);
if (dist < th) {
idxLast = i;
break;
}
}
// Convert to geo coordinates and append to waypoint manager.
const auto &ori = this->_origin;
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto &vertex = route[i];
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
_snakeWM.push_back(c);
}
// Do update.
this->_snakeWM.update(); // this can take a while (ca. 200ms)
......@@ -793,6 +845,7 @@ void WimaController::_threadFinishedHandler() {
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
#ifdef SNAKE_SHOW_TIME
auto end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): waypoints: "
......@@ -813,10 +866,11 @@ void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
void WimaController::_progressChangedHandler() {
const auto progress = this->_nemoInterface.progress();
if (this->_measurementArea.tiles().count() == progress.size()) {
if (this->_measurementArea.tiles()->count() == progress.size()) {
this->_measurementArea.progress() = std::move(progress);
emit nemoProgressChanged();
} else if (_localPlanDataValid) {
this->_updateRoute();
} else if (_planDataValid) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
}
}
......@@ -824,32 +878,64 @@ void WimaController::_progressChangedHandler() {
void WimaController::_enableSnakeChangedHandler() {
if (this->_enableSnake.rawValue().toBool()) {
qDebug() << "WimaController: enabling snake.";
_switchWaypointManager(_snakeWM);
this->_nemoInterface.start();
if (_planDataValid) {
if (!this->_nemoInterface.hasTileData(
this->_measurementArea.tileData())) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
}
}
} else {
qDebug() << "WimaController: disabling snake.";
_switchWaypointManager(_defaultWM);
this->_nemoInterface.stop();
}
}
void WimaController::_updateRoute() {
if (_localPlanDataValid && this->_joinedArea.coordinateList().size() > 0) {
// precondtion
if (_planDataValid && this->_joinedArea.coordinateList().size() > 0) {
const auto progress = this->_nemoInterface.progress();
const auto *tiles = this->_measurementArea.tiles();
// precondtion
if (tiles->count() > 0 && progress.size() == tiles->count() &&
this->_rawTransects.size() > 0 &&
this->_joinedArea.coordinateList().size() > 0) {
// Fetch tiles and convert to ENU.
this->_origin = this->_joinedArea.coordinateList().first();
this->_origin.setAltitude(0);
const auto &origin = this->_origin;
auto pTiles = std::make_shared<std::vector<snake::BoostPolygon>>();
std::size_t numTiles = 0;
for (int i = 0; i < progress.size(); ++i) {
if (progress[i] == 100) {
++numTiles;
const auto *obj = tiles->get(i);
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::BoostPolygon t;
snake::areaToEnu(origin, tile->coordinateList(), t);
pTiles->push_back(std::move(t));
} else {
return;
}
}
}
if (numTiles > 0) {
// Fetch safe area and convert to ENU.
auto safeArea = this->_joinedArea.coordinateList();
for (auto &v : safeArea) {
v.setAltitude(0);
}
const auto &origin = safeArea.front();
snake::BoostPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea, safeAreaENU);
const auto &depot = this->_serviceArea.depot();
const auto &geoTransects = this->_rawTransects;
const auto progess = this->_nemoInterface.progress();
auto pTiles = std::make_shared<QList<QList<QGeoCoordinate>>>();
auto generator = [origin, depot,
geoTransects](snake::Transects &transects) -> bool {
snake::BoostPolygon depotENU;
snake::BoostPoint depotENU;
snake::toENU(origin, depot, depotENU);
transects.push_back(snake::BoostLineString{depotENU});
// Fetch geo transects and convert to ENU.
const auto &geoTransects = this->_rawTransects;
auto pUnclippedTransects = std::make_shared<snake::Transects>();
for (auto &geoTransect : geoTransects) {
snake::BoostLineString t;
for (auto &geoVertex : geoTransect) {
......@@ -857,10 +943,69 @@ void WimaController::_updateRoute() {
snake::toENU(origin, geoVertex, v);
t.push_back(v);
}
transects.push_back(t);
pUnclippedTransects->push_back(t);
}
std::size_t minLength = 1; // meter
qWarning() << "using minLength dummy lkjfdslooij";
auto generator = [depotENU, pUnclippedTransects, pTiles,
minLength](snake::Transects &transects) -> bool {
// Convert transects to clipper path.
vector<ClipperLib::Path> transectsClipper;
for (const auto &t : *pUnclippedTransects) {
ClipperLib::Path path;
for (const auto &v : t) {
ClipperLib::IntPoint c{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)};
path.push_back(c);
}
transectsClipper.push_back(std::move(path));
}
// Add transects to clipper.
ClipperLib::Clipper clipper;
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
// Add tiles to clipper.
vector<ClipperLib::Path> tilesClipper;
for (const auto &t : *pTiles) {
ClipperLib::Path path;
for (const auto &v : t.outer()) {
path.push_back(ClipperLib::IntPoint{
static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
}
tilesClipper.push_back(std::move(path));
}
clipper.AddPaths(tilesClipper, ClipperLib::ptClip, true);
// Clip transects.
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Extract transects from PolyTree and convert them to
// BoostLineString
transects.push_back(snake::BoostLineString{depotENU});
for (const auto &child : clippedTransecs.Childs) {
snake::BoostLineString transect;
for (const auto &v : child->Contour) {
snake::BoostPoint c{static_cast<double>(v.X) / CLIPPER_SCALE,
static_cast<double>(v.Y) / CLIPPER_SCALE};
transect.push_back(c);
}
if (bg::length(transect) >= minLength) {
transects.push_back(std::move(transect));
}
}
if (transects.size() > 1) {
return true;
};
} else {
return false;
}
}; // generator
this->_routingThread.route(safeAreaENU, generator);
} else {
this->_storeRoute(RoutingThread::PtrRoutingData(new RoutingData()));
}
}
}
}
src/Wima/WimaController.h
View file @ c65f8eca
......@@ -6,12 +6,8 @@
#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"
//#include "Geometry/WimaArea.h"
//#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridorData.h"
//#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
//#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
......@@ -33,8 +29,6 @@ typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
class WimaController : public QObject {
Q_OBJECT
enum FileType { WimaFile, PlanFile };
public:
WimaController(QObject *parent = nullptr);
......@@ -213,7 +207,7 @@ private slots:
void _initSmartRTL();
void _smartRTLCleanUp(bool flying);
// Snake.
void _threadFinishedHandler();
void _storeRoute(RoutingThread::PtrRoutingData data);
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
void _switchToSnakeWaypointManager(QVariant variant);
void _progressChangedHandler();
......@@ -234,7 +228,7 @@ private:
WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid;
bool _planDataValid;
// Waypoint Managers.
WaypointManager::AreaInterface _areaInterface;
......@@ -283,6 +277,7 @@ private:
using StatusMap = std::map<int, QString>;
static StatusMap _nemoStatusMap;
RoutingThread _routingThread;
QGeoCoordinate _origin;
// Periodic tasks.
QTimer _eventTimer;
......
src/Wima/WimaPlanData.cc
View file @ c65f8eca
......@@ -89,6 +89,10 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData) {
}
}
void WimaPlanData::setTransects(const QList<QList<QGeoCoordinate>> &transects) {
_transects = transects;
}
void WimaPlanData::append(const QList<MissionItem *> &missionItems) {
for (auto *item : missionItems) {
item->setParent(this);
......@@ -110,6 +114,10 @@ const QList<const WimaAreaData *> &WimaPlanData::areaList() const {
return _areaList;
}
const QList<QList<QGeoCoordinate>> &WimaPlanData::transects() const {
return _transects;
}
const QList<MissionItem *> &WimaPlanData::missionItems() const {
return _missionItems;
}
......
src/Wima/WimaPlaner.cc
View file @ c65f8eca
......@@ -668,9 +668,13 @@ void WimaPlaner::synchronize() {
if (_wimaBridge != nullptr) {
if (readyForSynchronization()) {
auto planData = toPlanData();
if (planData) {
(void)_wimaBridge->setWimaPlanData(planData);
this->_synchronized = true;
emit synchronizedChanged();
} else {
qWarning("WimaPlaner::uploadToContainer(): error creating plan data.");
}
}
} else {
qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
......@@ -741,17 +745,18 @@ QSharedPointer<WimaPlanData> WimaPlaner::toPlanData() {
planData->append(WimaJoinedAreaData(_joinedArea));
// convert mission items to mavlink commands
QList<MissionItem *> missionItems;
if (_missionController && _missionController->visualItems()) {
int surveyIndex =
_missionController->visualItems()->indexOf(_TSComplexItem);
if (surveyIndex > 0) {
QList<MissionItem *> missionItems;
_TSComplexItem->appendMissionItems(missionItems, nullptr);
}
}
// store mavlink commands
planData->append(missionItems);
planData->setTransects(this->_TSComplexItem->rawTransects());
return planData;
}
}
return QSharedPointer<WimaPlanData>();
}
#ifndef NDEBUG
......
src/WimaView/ProgressIndicator.qml
View file @ c65f8eca
......@@ -5,8 +5,8 @@ import QtLocation 5.3
MapQuickItem {
id: root
width: 20
height: 20
width: 15
height: 15
anchorPoint.x: width/2
anchorPoint.y: height/2
......@@ -89,7 +89,7 @@ MapQuickItem {
// From 0 to angle
ctx.beginPath();
ctx.lineWidth = 3;
ctx.lineWidth = 1;
ctx.strokeStyle = root.secondaryColor;
ctx.arc(root.centerWidth,
root.centerHeight,
......
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