Commit 8ce30794 authored by Don Gagne's avatar Don Gagne
Browse files

Merge pull request #1848 from DonLakeFlyer/MissionEditor

Mission editor (WIP)
parents 0a770c97 69f27141
......@@ -144,6 +144,7 @@ INCLUDEPATH += \
src/input \
src/Joystick \
src/lib/qmapcontrol \
src/MissionEditor \
src/QmlControls \
src/uas \
src/ui \
......@@ -244,6 +245,7 @@ HEADERS += \
src/Joystick/JoystickManager.h \
src/LogCompressor.h \
src/MG.h \
src/MissionEditor/MissionEditor.h \
src/QGC.h \
src/QGCApplication.h \
src/QGCComboBox.h \
......@@ -381,6 +383,7 @@ SOURCES += \
src/Joystick/JoystickManager.cc \
src/LogCompressor.cc \
src/main.cc \
src/MissionEditor/MissionEditor.cc \
src/QGC.cc \
src/QGCApplication.cc \
src/QGCComboBox.cc \
......
......@@ -100,6 +100,7 @@
<file alias="QGroundControl/Controls/ModeSwitchDisplay.qml">src/QmlControls/ModeSwitchDisplay.qml</file>
<file alias="QGroundControl/Controls/MissionItemIndexLabel.qml">src/QmlControls/MissionItemIndexLabel.qml</file>
<file alias="QGroundControl/Controls/MissionItemSummary.qml">src/QmlControls/MissionItemSummary.qml</file>
<file alias="QGroundControl/Controls/MissionItemEditor.qml">src/QmlControls/MissionItemEditor.qml</file>
<!-- Vehicle Setup -->
<file alias="SetupView.qml">src/VehicleSetup/SetupView.qml</file>
......@@ -128,6 +129,7 @@
<file alias="FlightDisplayView.qml">src/FlightDisplay/FlightDisplayView.qml</file>
<file alias="FlightDisplayWidget.qml">src/FlightDisplay/FlightDisplayWidget.qml</file>
<file alias="MapDisplay.qml">src/ui/mapdisplay/MapDisplay.qml</file>
<file alias="MissionEditor.qml">src/MissionEditor/MissionEditor.qml</file>
<!-- FlightMap module -->
<file alias="QGroundControl/FlightMap/qmldir">src/FlightMap/qmldir</file>
......@@ -150,7 +152,7 @@
<file alias="QGroundControl/FlightMap/QGCWaypointEditor.qml">src/FlightMap/Widgets/QGCWaypointEditor.qml</file>
<!-- FlightMap MapQuickItems -->
<file alias="QGroundControl/FlightMap/MissionMapItem.qml">src/FlightMap/MapItems/MissionMapItem.qml</file>
<file alias="QGroundControl/FlightMap/MissionItemIndicator.qml">src/FlightMap/MapItems/MissionItemIndicator.qml</file>
<file alias="QGroundControl/FlightMap/VehicleMapItem.qml">src/FlightMap/MapItems/VehicleMapItem.qml</file>
</qresource>
......
......@@ -95,8 +95,10 @@ Item {
model: multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.missionItems : 0
delegate:
MissionMapItem {
missionItem: object
MissionItemIndicator {
label: object.sequenceNumber
isCurrentItem: object.isCurrentItem
coordinate: object.coordinate
}
}
......
......@@ -76,13 +76,13 @@ Map {
Menu {
id: mapTypeMenu
title: "Map Type..."
enabled: root.visible
enabled: _map.visible
ExclusiveGroup { id: currMapType }
function setCurrentMap(mapID) {
for (var i = 0; i < _map.supportedMapTypes.length; i++) {
if (mapID === _map.supportedMapTypes[i].name) {
_map.activeMapType = _map.supportedMapTypes[i]
multiVehicleManager.saveSetting(root.mapName + "/currentMapType", mapID);
multiVehicleManager.saveSetting(_map.mapName + "/currentMapType", mapID);
return;
}
}
......@@ -100,7 +100,7 @@ Map {
var mapID = ''
if (_map.supportedMapTypes.length > 0)
mapID = _map.activeMapType.name;
mapID = multiVehicleManager.loadSetting(root.mapName + "/currentMapType", mapID);
mapID = multiVehicleManager.loadSetting(_map.mapName + "/currentMapType", mapID);
for (var i = 0; i < _map.supportedMapTypes.length; i++) {
var name = _map.supportedMapTypes[i].name;
addMap(name, mapID === name);
......
......@@ -30,14 +30,14 @@ import QGroundControl.Vehicle 1.0
/// Marker for displaying a mission item on the map
MapQuickItem {
property var missionItem ///< Mission Item object
property alias label: _label.label
property alias isCurrentItem: _label.isCurrentItem
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
coordinate: missionItem.coordinate
sourceItem:
MissionItemIndexLabel {
missionItemIndex: missionItem.id
id: _label
}
}
......@@ -21,4 +21,4 @@ QGCWaypointEditor 1.0 QGCWaypointEditor.qml
# MapQuickItems
VehicleMapItem 1.0 VehicleMapItem.qml
MissionMapItem 1.0 MissionMapItem.qml
MissionItemIndicator 1.0 MissionItemIndicator.qml
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#include "MissionEditor.h"
#include "ScreenToolsController.h"
#include <QQmlContext>
#include <QQmlEngine>
#include <QSettings>
const char* MissionEditor::_settingsGroup = "MissionEditor";
MissionEditor::MissionEditor(QWidget *parent)
: QGCQmlWidgetHolder(parent)
{
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
// Get rid of layout default margins
QLayout* pl = layout();
if(pl) {
pl->setContentsMargins(0,0,0,0);
}
#ifndef __android__
setMinimumWidth( 31 * ScreenToolsController::defaultFontPixelSize_s());
setMinimumHeight(33 * ScreenToolsController::defaultFontPixelSize_s());
#endif
setContextPropertyObject("controller", this);
setSource(QUrl::fromUserInput("qrc:/qml/MissionEditor.qml"));
}
MissionEditor::~MissionEditor()
{
}
void MissionEditor::saveSetting(const QString &name, const QString& value)
{
QSettings settings;
settings.beginGroup(_settingsGroup);
settings.setValue(name, value);
}
QString MissionEditor::loadSetting(const QString &name, const QString& defaultValue)
{
QSettings settings;
settings.beginGroup(_settingsGroup);
return settings.value(name, defaultValue).toString();
}
void MissionEditor::addMissionItem(QGeoCoordinate coordinate)
{
MissionItem * newItem = new MissionItem(this, _missionItems.count(), coordinate);
if (_missionItems.count() == 0) {
newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
}
qDebug() << "MissionItem" << newItem->coordinate();
_missionItems.append(newItem);
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef MissionEditor_H
#define MissionEditor_H
#include "QGCQmlWidgetHolder.h"
#include "QmlObjectListModel.h"
class MissionEditor : public QGCQmlWidgetHolder
{
Q_OBJECT
public:
MissionEditor(QWidget* parent = NULL);
~MissionEditor();
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel CONSTANT)
Q_INVOKABLE void addMissionItem(QGeoCoordinate coordinate);
Q_INVOKABLE void saveSetting (const QString &key, const QString& value);
Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue);
// Property accessors
QmlObjectListModel* missionItemsModel(void) { return &_missionItems; }
private:
QmlObjectListModel _missionItems;
static const char* _settingsGroup;
};
#endif
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
import QtQuick 2.4
import QtQuick.Controls 1.3
import QtQuick.Dialogs 1.2
import QtLocation 5.3
import QtPositioning 5.3
import QGroundControl.FlightMap 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0
/// Mission Editor
Item {
// For some reason we can't have FlightMap as the top level control. If you do that it doesn't draw.
// So we have an Item at the top to work around that.
readonly property real _defaultLatitude: 37.803784
readonly property real _defaultLongitude: -122.462276
readonly property int _decimalPlaces: 7
FlightMap {
id: editorMap
anchors.fill: parent
mapName: "MissionEditor"
latitude: _defaultLatitude
longitude: _defaultLongitude
QGCLabel { text: "WIP: Non functional"; font.pixelSize: ScreenTools.largeFontPixelSize }
MouseArea {
anchors.fill: parent
onClicked: {
var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y))
coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces)
coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces)
coordinate.altitude = 0
controller.addMissionItem(coordinate)
}
}
// Add the mission items to the map
MapItemView {
model: controller.missionItems
delegate:
MissionItemIndicator {
label: object.sequenceNumber
isCurrentItem: object.isCurrentItem
coordinate: object.coordinate
Component.onCompleted: console.log("Indicator", object.coordinate)
}
}
// Mission item list
ListView {
id: missionItemSummaryList
anchors.margins: ScreenTools.defaultFontPixelHeight
anchors.top: parent.top
anchors.bottom: editorMap.mapWidgets.top
anchors.right: parent.right
width: ScreenTools.defaultFontPixelWidth * 30
spacing: ScreenTools.defaultFontPixelHeight / 2
orientation: ListView.Vertical
model: controller.missionItems
property real _maxItemHeight: 0
delegate:
MissionItemEditor {
missionItem: object
}
}
Column {
id: controlWidgets
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.right: parent.left
anchors.bottom: parent.top
spacing: ScreenTools.defaultFontPixelWidth / 2
QGCButton {
id: addMode
text: "+"
checkable: true
}
}
}
}
......@@ -34,41 +34,43 @@ This file is part of the QGROUNDCONTROL project
#include "MissionItem.h"
MissionItem::MissionItem(
QObject *parent,
quint16 id,
double x,
double y,
double z,
double param1,
double param2,
double param3,
double param4,
bool autocontinue,
bool current,
int frame,
int action,
const QString& description
)
const MissionItem::MavCmd2Name_t MissionItem::_rgMavCmd2Name[_cMavCmd2Name] = {
{ MAV_CMD_NAV_WAYPOINT, "Waypoint" },
{ MAV_CMD_NAV_LOITER_UNLIM, "Loiter" },
{ MAV_CMD_NAV_LOITER_TURNS, "Loiter (turns)" },
{ MAV_CMD_NAV_LOITER_TIME, "Loiter (unlimited)" },
{ MAV_CMD_NAV_RETURN_TO_LAUNCH, "Return Home" },
{ MAV_CMD_NAV_LAND, "Land" },
{ MAV_CMD_NAV_TAKEOFF, "Takeoff" },
{ MAV_CMD_CONDITION_DELAY, "Delay" },
{ MAV_CMD_DO_JUMP, "Jump To Command" },
};
MissionItem::MissionItem(QObject* parent,
int sequenceNumber,
QGeoCoordinate coordinate,
double param1,
double param2,
double param3,
double param4,
bool autocontinue,
bool isCurrentItem,
int frame,
int action)
: QObject(parent)
, _id(id)
, _x(x)
, _y(y)
, _z(z)
, _sequenceNumber(sequenceNumber)
, _coordinate(coordinate)
, _yaw(param4)
, _frame(frame)
, _action(action)
, _autocontinue(autocontinue)
, _current(current)
, _isCurrentItem(isCurrentItem)
, _orbit(param3)
, _param1(param1)
, _param2(param2)
, _name(QString("WP%1").arg(id, 2, 10, QChar('0')))
, _description(description)
, _reachedTime(0)
{
connect(this, &MissionItem::latitudeChanged, this, &MissionItem::coordinateChanged);
connect(this, &MissionItem::longitudeChanged, this, &MissionItem::coordinateChanged);
}
MissionItem::MissionItem(const MissionItem& other)
......@@ -83,21 +85,18 @@ MissionItem::~MissionItem()
const MissionItem& MissionItem::operator=(const MissionItem& other)
{
_id = other.getId();
_x = other.getX();
_y = other.getY();
_z = other.getZ();
_yaw = other.getYaw();
_frame = other.getFrame();
_action = other.getAction();
_autocontinue = other.getAutoContinue();
_current = other.getCurrent();
_orbit = other.getParam3();
_param1 = other.getParam1();
_param2 = other.getParam2();
_name = other.getName();
_description = other.getDescription();
_reachedTime = other.getReachedTime();
_sequenceNumber = other._sequenceNumber;
_isCurrentItem = other._isCurrentItem;
_coordinate = other._coordinate;
_yaw = other._yaw;
_frame = other._frame;
_action = other._action;
_autocontinue = other._autocontinue;
_orbit = other._orbit;
_param1 = other._param1;
_param2 = other._param2;
_reachedTime = other._reachedTime;
return *this;
}
......@@ -109,43 +108,42 @@ bool MissionItem::isNavigationType()
void MissionItem::save(QTextStream &saveStream)
{
QString position("%1\t%2\t%3");
position = position.arg(_x, 0, 'g', 18);
position = position.arg(_y, 0, 'g', 18);
position = position.arg(_z, 0, 'g', 18);
position = position.arg(x(), 0, 'g', 18);
position = position.arg(y(), 0, 'g', 18);
position = position.arg(z(), 0, 'g', 18);
QString parameters("%1\t%2\t%3\t%4");
parameters = parameters.arg(_param1, 0, 'g', 18).arg(_param2, 0, 'g', 18).arg(_orbit, 0, 'g', 18).arg(_yaw, 0, 'g', 18);
// FORMAT: <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LONGITUDE> <PARAM6/Y/LATITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE> <DESCRIPTION>
// as documented here: http://qgroundcontrol.org/waypoint_protocol
saveStream << this->getId() << "\t" << this->getCurrent() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << parameters << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n";
saveStream << this->sequenceNumber() << "\t" << this->isCurrentItem() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << parameters << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n";
}
bool MissionItem::load(QTextStream &loadStream)
{
const QStringList &wpParams = loadStream.readLine().split("\t");
if (wpParams.size() == 12) {
_id = wpParams[0].toInt();
_current = (wpParams[1].toInt() == 1 ? true : false);
setSequenceNumber(wpParams[0].toInt());
setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
_frame = (MAV_FRAME) wpParams[2].toInt();
_action = (MAV_CMD) wpParams[3].toInt();
_param1 = wpParams[4].toDouble();
_param2 = wpParams[5].toDouble();
_orbit = wpParams[6].toDouble();
_yaw = wpParams[7].toDouble();
_x = wpParams[8].toDouble();
_y = wpParams[9].toDouble();
_z = wpParams[10].toDouble();
setYaw(wpParams[7].toDouble());
setLatitude(wpParams[8].toDouble());
setLongitude(wpParams[9].toDouble());
setAltitude(wpParams[10].toDouble());
_autocontinue = (wpParams[11].toInt() == 1 ? true : false);
//_description = wpParams[12];
return true;
}
return false;
}
void MissionItem::setId(quint16 id)
void MissionItem::setSequenceNumber(int sequenceNumber)
{
_id = id;
_name = QString("WP%1").arg(id, 2, 10, QChar('0'));
_sequenceNumber = sequenceNumber;
emit sequenceNumberChanged(_sequenceNumber);
emit changed(this);
}
......@@ -153,8 +151,7 @@ void MissionItem::setX(double x)
{
if (!isinf(x) && !isnan(x) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
_x = x;
emit changed(this);
setLatitude(x);
}
}
......@@ -162,8 +159,7 @@ void MissionItem::setY(double y)
{
if (!isinf(y) && !isnan(y) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
_y = y;
emit changed(this);
setLongitude(y);
}
}
......@@ -171,49 +167,38 @@ void MissionItem::setZ(double z)
{
if (!isinf(z) && !isnan(z) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
{
_z = z;
emit changed(this);
setAltitude(z);
}
}
void MissionItem::setLatitude(double lat)
{
if (_x != lat && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
if (_coordinate.latitude() != lat && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
_x = lat;
_coordinate.setLatitude(lat);
emit changed(this);
emit coordinateChanged();
emit coordinateChanged(coordinate());
}
}
void MissionItem::setLongitude(double lon)
{
if (_y != lon && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
if (_coordinate.longitude() != lon && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
_y = lon;
_coordinate.setLongitude(lon);
emit changed(this);
emit coordinateChanged();
emit coordinateChanged(coordinate());
}
}
void MissionItem::setAltitude(double altitude)
{
if (_z != altitude && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
if (_coordinate.altitude() != altitude && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
_z = altitude;
emit changed(this);
emit valueStringsChanged(valueStrings());
emit coordinateChanged();
}
}
void MissionItem::setYaw(int yaw)
{
if (_yaw != yaw)
{
_yaw = yaw;
_coordinate.setAltitude(altitude);
emit changed(this);
emit valueStringsChanged(valueStrings());
emit coordinateChanged(coordinate());
}
}
......@@ -222,6 +207,7 @@ void MissionItem::setYaw(double yaw)
if (_yaw != yaw)
{
_yaw = yaw;
emit yawChanged(yaw);
emit changed(this);
emit valueStringsChanged(valueStrings());
}
......@@ -242,7 +228,7 @@ void MissionItem::setAction(int /*MAV_CMD*/ action)
emit changed(this);
emit commandNameChanged(commandName());
emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_action);
emit hasCoordinateChanged(hasCoordinate());
emit specifiesCoordinateChanged(specifiesCoordinate());
emit valueLabelsChanged(valueLabels());
emit valueStringsChanged(valueStrings());
}
......@@ -264,13 +250,11 @@ void MissionItem::setAutocontinue(bool autoContinue)
}
}
void MissionItem::setCurrent(bool current)
void MissionItem::setIsCurrentItem(bool isCurrentItem)
{
if (_current != current)
{
_current = current;
// The current waypoint index is handled by the list
// and not part of the individual waypoint update state
if (_isCurrentItem != isCurrentItem) {
_isCurrentItem = isCurrentItem;
emit isCurrentItemChanged(isCurrentItem);
}
}
......@@ -326,8 +310,8 @@ void MissionItem::setParam4(double param4)
void MissionItem::setParam5(double param5)
{
if (_x != param5) {
_x = param5;
if (_coordinate.latitude() != param5) {
_coordinate.setLatitude(param5);
emit changed(this);
emit valueStringsChanged(valueStrings());
}
......@@ -335,8 +319,8 @@ void MissionItem::setParam5(double param5)
void MissionItem::setParam6(double param6)
{
if (_y != param6) {
_y = param6;
if (_coordinate.longitude() != param6) {
_coordinate.setLongitude(param6);
emit changed(this);
emit valueStringsChanged(valueStrings());
}
......@@ -344,8 +328,8 @@ void MissionItem::setParam6(double param6)
void MissionItem::setParam7(double param7)
{
if (_z != param7) {
_z = param7;
if (_coordinate.altitude() != param7) {
_coordinate.setAltitude(param7);
emit valueStringsChanged(valueStrings());
emit changed(this);
}
......@@ -387,7 +371,7 @@ void MissionItem::setTurns(int turns)
}
}
bool MissionItem::hasCoordinate(void)
bool MissionItem::specifiesCoordinate(void) const
{
switch (_action) {
case MAV_CMD_NAV_WAYPOINT:
......@@ -402,11 +386,6 @@ bool MissionItem::hasCoordinate(void)
}
}
QGeoCoordinate MissionItem::coordinate(void)
{
return QGeoCoordinate(_x, _y);
}
QString MissionItem::commandName(void)
{
QString type;
......@@ -507,7 +486,7 @@ QStringList MissionItem::valueStrings(void)
switch (_action) {
case MAV_CMD_NAV_WAYPOINT:
list << _oneDecimalString(_z) << _oneDecimalString(_yaw * (180.0 / M_PI)) << _oneDecimalString(_param2) << _oneDecimalString(_param1);
list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(_yaw * (180.0 / M_PI)) << _oneDecimalString(_param2) << _oneDecimalString(_param1);
break;
case MAV_CMD_NAV_LOITER_UNLIM:
list << _oneDecimalString(_yaw * (180.0 / M_PI)) << _oneDecimalString(_orbit);
......@@ -521,10 +500,10 @@ QStringList MissionItem::valueStrings(void)
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
break;
case MAV_CMD_NAV_LAND:
list << _oneDecimalString(_z) << _oneDecimalString(_yaw * (180.0 / M_PI));
list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(_yaw * (180.0 / M_PI));
break;
case MAV_CMD_NAV_TAKEOFF:
list << _oneDecimalString(_z) << _oneDecimalString(_yaw * (180.0 / M_PI)) << _oneDecimalString(_param1);
list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(_yaw * (180.0 / M_PI)) << _oneDecimalString(_param1);
break;
case MAV_CMD_CONDITION_DELAY:
list << _oneDecimalString(_param1);
......@@ -537,4 +516,42 @@ QStringList MissionItem::valueStrings(void)
}
return list;
}
\ No newline at end of file
}
void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
_coordinate = coordinate;
emit coordinateChanged(coordinate);
emit changed(this);
}
QStringList MissionItem::commandNames(void) {
QStringList list;
for (int i=0; i<_cMavCmd2Name; i++) {
list += _rgMavCmd2Name[i].name;
}
return list;
}
int MissionItem::commandByIndex(void)
{
for (int i=0; i<_cMavCmd2Name; i++) {
if (_rgMavCmd2Name[i].command == _action) {
return i;
}
}
return -1;
}
void MissionItem::setCommandByIndex(int index)
{
if (index < 0 || index >= _cMavCmd2Name) {
qWarning() << "Invalid index" << index;
return;
}
setCommand((MavlinkQmlSingleton::Qml_MAV_CMD)_rgMavCmd2Name[index].command);
}
......@@ -39,76 +39,84 @@ class MissionItem : public QObject
Q_OBJECT
public:
MissionItem(
QObject *parent = 0,
quint16 id = 0,
double x = 0.0,
double y = 0.0,
double z = 0.0,
double param1 = 0.0,
double param2 = 0.0,
double param3 = 0.0,
double param4 = 0.0,
bool autocontinue = true,
bool current = false,
int frame = MAV_FRAME_GLOBAL,
int action = MAV_CMD_NAV_WAYPOINT,
const QString& description=QString(""));
MissionItem(QObject *parent = 0,
int sequenceNumber = 0,
QGeoCoordinate coordiante = QGeoCoordinate(),
double param1 = 0.0,
double param2 = 0.0,
double param3 = 0.0,
double param4 = 0.0,
bool autocontinue = true,
bool isCurrentItem = false,
int frame = MAV_FRAME_GLOBAL,
int action = MAV_CMD_NAV_WAYPOINT);
MissionItem(const MissionItem& other);
~MissionItem();
const MissionItem& operator=(const MissionItem& other);
Q_PROPERTY(bool hasCoordinate READ hasCoordinate NOTIFY hasCoordinateChanged)
Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged)
Q_PROPERTY(QString commandName READ commandName NOTIFY commandNameChanged)
Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
Q_PROPERTY(int sequenceNumber READ sequenceNumber WRITE setSequenceNumber NOTIFY sequenceNumberChanged)
Q_PROPERTY(bool isCurrentItem READ isCurrentItem WRITE setIsCurrentItem NOTIFY isCurrentItemChanged)
Q_PROPERTY(bool specifiesCoordinate READ specifiesCoordinate NOTIFY specifiesCoordinateChanged)
Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged)
Q_PROPERTY(double yaw READ yaw WRITE setYaw NOTIFY yawChanged)
Q_PROPERTY(QStringList commandNames READ commandNames CONSTANT)
Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged)
Q_PROPERTY(QStringList valueLabels READ valueLabels NOTIFY commandChanged)
Q_PROPERTY(QStringList valueStrings READ valueStrings NOTIFY valueStringsChanged)
Q_PROPERTY(int commandByIndex READ commandByIndex WRITE setCommandByIndex NOTIFY commandChanged)
Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
// Property accesors
int sequenceNumber(void) const { return _sequenceNumber; }
void setSequenceNumber(int sequenceNumber);
bool isCurrentItem(void) const { return _isCurrentItem; }
void setIsCurrentItem(bool isCurrentItem);
Q_PROPERTY(double longitude READ longitude NOTIFY longitudeChanged)
Q_PROPERTY(double latitude READ latitude NOTIFY latitudeChanged)
Q_PROPERTY(double altitude READ altitude NOTIFY altitudeChanged)
Q_PROPERTY(quint16 id READ id CONSTANT)
bool specifiesCoordinate(void) const;
Q_PROPERTY(QStringList valueLabels READ valueLabels NOTIFY commandChanged)
Q_PROPERTY(QStringList valueStrings READ valueStrings NOTIFY valueStringsChanged)
QGeoCoordinate coordinate(void) const { return _coordinate; }
void setCoordinate(const QGeoCoordinate& coordinate);
QStringList commandNames(void);
QString commandName(void);
double latitude() { return _x; }
double longitude() { return _y; }
double altitude() { return _z; }
quint16 id() { return _id; }
int commandByIndex(void);
void setCommandByIndex(int index);
MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)getAction(); };
void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) { setAction(command); }
QStringList valueLabels(void);
QStringList valueStrings(void);
// C++ only methods
quint16 getId() const {
return _id;
}
double getX() const {
return _x;
}
double getY() const {
return _y;
}
double getZ() const {
return _z;
}
double getLatitude() const {
return _x;
}
double getLongitude() const {
return _y;
}
double getAltitude() const {
return _z;
}
double getYaw() const {
return _yaw;
}
double latitude(void) const { return _coordinate.latitude(); }
double longitude(void) const { return _coordinate.longitude(); }
double altitude(void) const { return _coordinate.altitude(); }
void setLatitude(double latitude);
void setLongitude(double longitude);
void setAltitude(double altitude);
double x(void) const { return latitude(); }
double y(void) const { return longitude(); }
double z(void) const { return altitude(); }
void setX(double x);
void setY(double y);
void setZ(double z);
double yaw(void) const { return _yaw; }
void setYaw(double yaw);
bool getAutoContinue() const {
return _autocontinue;
}
bool getCurrent() const {
return _current;
}
double getLoiterOrbit() const {
return _orbit;
}
......@@ -128,16 +136,16 @@ public:
return _orbit;
}
double getParam4() const {
return _yaw;
return yaw();
}
double getParam5() const {
return _x;
return latitude();
}
double getParam6() const {
return _y;
return longitude();
}
double getParam7() const {
return _z;
return altitude();
}
int getTurns() const {
return _param1;
......@@ -150,13 +158,6 @@ public:
int getAction() const {
return _action;
}
const QString& getName() const {
return _name;
}
const QString& getDescription() const {
return _description;
}
/** @brief Returns true if x, y, z contain reasonable navigation data */
bool isNavigationType();
......@@ -166,47 +167,24 @@ public:
void save(QTextStream &saveStream);
bool load(QTextStream &loadStream);
bool hasCoordinate(void);
QGeoCoordinate coordinate(void);
QString commandName(void);
signals:
void sequenceNumberChanged(int sequenceNumber);
void specifiesCoordinateChanged(bool specifiesCoordinate);
void isCurrentItemChanged(bool isCurrentItem);
void coordinateChanged(const QGeoCoordinate& coordinate);
void yawChanged(double yaw);
/** @brief Announces a change to the waypoint data */
void changed(MissionItem* wp);
MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)getAction(); };
void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) { setAction(command); }
QStringList valueLabels(void);
QStringList valueStrings(void);
void commandNameChanged(QString type);
void commandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
void valueLabelsChanged(QStringList valueLabels);
void valueStringsChanged(QStringList valueStrings);
protected:
quint16 _id;
double _x;
double _y;
double _z;
double _yaw;
int _frame;
int _action;
bool _autocontinue;
bool _current;
double _orbit;
double _param1;
double _param2;
int _turns;
QString _name;
QString _description;
quint64 _reachedTime;
public:
void setId (quint16 _id);
void setX (double _x);
void setY (double _y);
void setZ (double _z);
void setLatitude (double lat);
void setLongitude (double lon);
void setAltitude (double alt);
/** @brief Yaw angle in COMPASS DEGREES: 0-360 */
void setYaw (int _yaw);
/** @brief Yaw angle in COMPASS DEGREES: 0-360 */
void setYaw (double _yaw);
/** @brief Set the waypoint action */
void setAction (int _action);
void setFrame (int _frame);
......@@ -234,22 +212,30 @@ public:
emit changed(this);
}
signals:
/** @brief Announces a change to the waypoint data */
void changed(MissionItem* wp);
void latitudeChanged ();
void longitudeChanged ();
void altitudeChanged ();
void hasCoordinateChanged(bool hasCoordinate);
void coordinateChanged(void);
void commandNameChanged(QString type);
void commandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
void valueLabelsChanged(QStringList valueLabels);
void valueStringsChanged(QStringList valueStrings);
private:
QString _oneDecimalString(double value);
private:
typedef struct {
MAV_CMD command;
const char* name;
} MavCmd2Name_t;
int _sequenceNumber;
QGeoCoordinate _coordinate;
double _yaw;
int _frame;
int _action;
bool _autocontinue;
bool _isCurrentItem;
double _orbit;
double _param1;
double _param2;
int _turns;
quint64 _reachedTime;
static const int _cMavCmd2Name = 9;
static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name];
};
#endif
import QtQuick 2.2
import QtQuick.Controls 1.2
import QtQuick.Controls.Styles 1.2
import QGroundControl.ScreenTools 1.0
import QGroundControl.Vehicle 1.0
/// Mission item edit control
Rectangle {
property var missionItem
width: _editFieldWidth + (ScreenTools.defaultFontPixelWidth * 10)
height: _valueColumn.y + _valueColumn.height + (radius / 2)
border.width: 2
border.color: "white"
color: "white"
radius: ScreenTools.defaultFontPixelWidth
readonly property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 13
MissionItemIndexLabel {
id: _label
anchors.top: parent.top
anchors.right: parent.right
isCurrentItem: missionItem.isCurrentItem
label: missionItem.sequenceNumber
}
QGCComboBox {
id: _commandCombo
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.right: _label.left
anchors.top: parent.top
currentIndex: missionItem.commandByIndex
model: missionItem.commandNames
onActivated: missionItem.commandByIndex = index
}
Column {
id: _coordinateColumn
anchors.left: parent.left
anchors.right: parent.right
visible: missionItem.specifiesCoordinate
}
QGCTextField {
id: _latitudeField
anchors.margins: parent.radius / 2
anchors.top: _commandCombo.bottom
anchors.right: parent.right
width: _editFieldWidth
text: missionItem.coordinate.latitude
visible: missionItem.specifiesCoordinate
onAccepted: missionItem.coordinate.latitude = text
}
QGCTextField {
id: _longitudeField
anchors.margins: parent.radius / 2
anchors.top: _latitudeField.bottom
anchors.right: parent.right
width: _editFieldWidth
text: missionItem.coordinate.longitude
visible: missionItem.specifiesCoordinate
onAccepted: missionItem.coordinate.longtitude = text
}
QGCTextField {
id: _altitudeField
anchors.margins: parent.radius / 2
anchors.top: _longitudeField.bottom
anchors.right: parent.right
width: _editFieldWidth
text: missionItem.coordinate.altitude
visible: missionItem.specifiesCoordinate
showUnits: true
unitsLabel: "meters"
onAccepted: missionItem.coordinate.altitude = text
}
QGCLabel {
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.baseline: _latitudeField.baseline
color: "black"
text: "Lat:"
visible: missionItem.specifiesCoordinate
}
QGCLabel {
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.baseline: _longitudeField.baseline
color: "black"
text: "Long:"
visible: missionItem.specifiesCoordinate
}
QGCLabel {
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.baseline: _altitudeField.baseline
color: "black"
text: "Alt:"
visible: missionItem.specifiesCoordinate
}
Column {
id: _valueColumn
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.right: parent.right
anchors.top: missionItem.specifiesCoordinate ? _altitudeField.bottom : _commandCombo.bottom
Repeater {
model: missionItem.valueLabels
QGCLabel {
color: "black"
text: modelData
}
}
}
Column {
anchors.margins: parent.radius / 2
anchors.left: parent.left
anchors.right: parent.right
anchors.top: _valueColumn.top
Repeater {
model: missionItem.valueStrings
QGCLabel {
width: _valueColumn.width
color: "black"
text: modelData
horizontalAlignment: Text.AlignRight
}
}
}
}
......@@ -5,20 +5,21 @@ import QtQuick.Controls.Styles 1.2
import QGroundControl.ScreenTools 1.0
Rectangle {
property int missionItemIndex ///< Index to show in label
property alias label: _label.text
property bool isCurrentItem: false
width: ScreenTools.defaultFontPixelHeight * 1.5
height: width
radius: width / 2
border.width: 2
border.color: "white"
color: "orange"
color: isCurrentItem ? "green" : "orange"
QGCLabel {
id: _label
anchors.fill: parent
horizontalAlignment: Text.AlignHCenter
verticalAlignment: Text.AlignVCenter
color: "white"
text: missionItemIndex
}
}
......@@ -7,7 +7,7 @@ import QGroundControl.Vehicle 1.0
/// Mission item summary display control
Rectangle {
property var missionItem ///< Mission Item object
property var missionItem ///< Mission Item object
width: ScreenTools.defaultFontPixelWidth * 15
height: valueColumn.height + radius
......@@ -17,9 +17,11 @@ Rectangle {
radius: ScreenTools.defaultFontPixelWidth
MissionItemIndexLabel {
id: _indexLabel
anchors.top: parent.top
anchors.right: parent.right
missionItemIndex: missionItem.id
isCurrentItem: missionItem.isCurrentItem
label: missionItem.sequenceNumber
}
Column {
......
......@@ -28,4 +28,5 @@ QGCViewMessage 1.0 QGCViewMessage.qml
MissionItemIndexLabel 1.0 MissionItemIndexLabel.qml
MissionItemSummary 1.0 MissionItemSummary.qml
MissionItemEditor 1.0 MissionItemEditor.qml
......@@ -708,7 +708,7 @@ void Vehicle::_updateWaypointViewOnly(int, MissionItem* /*wp*/)
/*
bool changed = false;
for(int i = 0; i < _waypoints.count(); i++) {
if(_waypoints[i].getId() == wp->getId()) {
if(_waypoints[i].sequenceNumber() == wp->sequenceNumber()) {
_waypoints[i] = *wp;
changed = true;
break;
......
......@@ -116,9 +116,9 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x,
Q_UNUSED(time);
if (waypointsEditable.count() > 0 && !currentWaypointEditable.isNull() && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU))
{
double xdiff = x-currentWaypointEditable->getX();
double ydiff = y-currentWaypointEditable->getY();
double zdiff = z-currentWaypointEditable->getZ();
double xdiff = x-currentWaypointEditable->x();
double ydiff = y-currentWaypointEditable->y();
double zdiff = z-currentWaypointEditable->z();
double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
emit waypointDistanceChanged(dist);
}
......@@ -196,37 +196,32 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id) {
MissionItem *lwp_vo = new MissionItem(
NULL,
wp->seq, wp->x,
wp->y,
wp->z,
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
MissionItem *lwp_vo = new MissionItem(NULL,
wp->seq,
QGeoCoordinate(wp->x, wp->y, wp->z),
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
addWaypointViewOnly(lwp_vo);
if (read_to_edit == true) {
MissionItem *lwp_ed = new MissionItem(
NULL,
wp->seq,
wp->x,
wp->y,
wp->z,
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
MissionItem *lwp_ed = new MissionItem(NULL,
wp->seq,
QGeoCoordinate(wp->x, wp->y, wp->z),
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
addWaypointEditable(lwp_ed, false);
if (wp->current == 1) currentWaypointEditable = lwp_ed;
}
......@@ -258,20 +253,17 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
&& wp->seq < waypointsViewOnly.size() && waypointsViewOnly[wp->seq]->getAction()) {
// accept single sent waypoints because they can contain updates about remaining DO_JUMP repetitions
// but only update view only side
MissionItem *lwp_vo = new MissionItem(
NULL,
wp->seq,
wp->x,
wp->y,
wp->z,
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
MissionItem *lwp_vo = new MissionItem(NULL,
wp->seq,
QGeoCoordinate(wp->x, wp->y, wp->z),
wp->param1,
wp->param2,
wp->param3,
wp->param4,
wp->autocontinue,
wp->current,
(MAV_FRAME) wp->frame,
(MAV_CMD) wp->command);
waypointsViewOnly.replace(wp->seq, lwp_vo);
emit waypointViewOnlyListChanged();
......@@ -386,10 +378,10 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
// update the local main storage
if (wpc->seq < waypointsViewOnly.size()) {
for(int i = 0; i < waypointsViewOnly.size(); i++) {
if (waypointsViewOnly[i]->getId() == wpc->seq) {
waypointsViewOnly[i]->setCurrent(true);
if (waypointsViewOnly[i]->sequenceNumber() == wpc->seq) {
waypointsViewOnly[i]->setIsCurrentItem(true);
} else {
waypointsViewOnly[i]->setCurrent(false);
waypointsViewOnly[i]->setIsCurrentItem(false);
}
}
}
......@@ -450,10 +442,10 @@ int UASWaypointManager::setCurrentEditable(quint16 seq)
if(current_state == WP_IDLE) {
//update local main storage
for (int i = 0; i < waypointsEditable.count(); i++) {
if (waypointsEditable[i]->getId() == seq) {
waypointsEditable[i]->setCurrent(true);
if (waypointsEditable[i]->sequenceNumber() == seq) {
waypointsEditable[i]->setIsCurrentItem(true);
} else {
waypointsEditable[i]->setCurrent(false);
waypointsEditable[i]->setIsCurrentItem(false);
}
}
......@@ -490,10 +482,10 @@ void UASWaypointManager::addWaypointEditable(MissionItem *wp, bool enforceFirstA
QGCMessageBox::critical(tr("MissionItem Manager"), _offlineEditingModeMessage);
}
wp->setId(waypointsEditable.count());
wp->setSequenceNumber(waypointsEditable.count());
if (enforceFirstActive && waypointsEditable.count() == 0)
{
wp->setCurrent(true);
wp->setIsCurrentItem(true);
currentWaypointEditable = wp;
}
waypointsEditable.insert(waypointsEditable.count(), wp);
......@@ -515,13 +507,13 @@ MissionItem* UASWaypointManager::createWaypoint(bool enforceFirstActive)
}
MissionItem* wp = new MissionItem();
wp->setId(waypointsEditable.count());
wp->setSequenceNumber(waypointsEditable.count());
wp->setFrame((MAV_FRAME)getFrameRecommendation());
wp->setAltitude(getAltitudeRecommendation());
wp->setAcceptanceRadius(getAcceptanceRadiusRecommendation());
if (enforceFirstActive && waypointsEditable.count() == 0)
{
wp->setCurrent(true);
wp->setIsCurrentItem(true);
currentWaypointEditable = wp;
}
waypointsEditable.append(wp);
......@@ -538,15 +530,15 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
{
MissionItem *t = waypointsEditable[seq];
if (t->getCurrent() == true) //trying to remove the current waypoint
if (t->isCurrentItem() == true) //trying to remove the current waypoint
{
if (seq+1 < waypointsEditable.count()) // setting the next waypoint as current
{
waypointsEditable[seq+1]->setCurrent(true);
waypointsEditable[seq+1]->setIsCurrentItem(true);
}
else if (seq-1 >= 0) // if deleting the last on the list, then setting the previous waypoint as current
{
waypointsEditable[seq-1]->setCurrent(true);
waypointsEditable[seq-1]->setIsCurrentItem(true);
}
}
......@@ -556,7 +548,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
for(int i = seq; i < waypointsEditable.count(); i++)
{
waypointsEditable[i]->setId(i);
waypointsEditable[i]->setSequenceNumber(i);
}
emit waypointEditableListChanged();
......@@ -575,7 +567,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
for (int i = cur_seq; i < new_seq; i++)
{
waypointsEditable[i] = waypointsEditable[i+1];
waypointsEditable[i]->setId(i);
waypointsEditable[i]->setSequenceNumber(i);
}
}
else
......@@ -583,11 +575,11 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
for (int i = cur_seq; i > new_seq; i--)
{
waypointsEditable[i] = waypointsEditable[i-1];
waypointsEditable[i]->setId(i);
waypointsEditable[i]->setSequenceNumber(i);
}
}
waypointsEditable[new_seq] = t;
waypointsEditable[new_seq]->setId(new_seq);
waypointsEditable[new_seq]->setSequenceNumber(new_seq);
emit waypointEditableListChanged();
emit waypointEditableListChanged(uasid);
......@@ -607,7 +599,7 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile)
for (int i = 0; i < waypointsEditable.count(); i++)
{
waypointsEditable[i]->setId(i);
waypointsEditable[i]->setSequenceNumber(i);
waypointsEditable[i]->save(out);
}
file.close();
......@@ -642,7 +634,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
{
//Use the existing function to add waypoints to the map instead of doing it manually
//Indeed, we should connect our waypoints to the map in order to synchronize them
//t->setId(waypointsEditable.count());
//t->setSequenceNumber(waypointsEditable.count());
// waypointsEditable.insert(waypointsEditable.count(), t);
addWaypointEditable(t, false);
}
......@@ -956,9 +948,9 @@ void UASWaypointManager::goToWaypoint(MissionItem *wp)
mission.frame = wp->getFrame();
mission.command = wp->getAction();
mission.seq = 0; // don't read out the sequence number of the waypoint class
mission.x = wp->getX();
mission.y = wp->getY();
mission.z = wp->getZ();
mission.x = wp->x();
mission.y = wp->y();
mission.z = wp->z();
mavlink_message_t message;
mission.target_system = uasid;
mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
......@@ -1001,7 +993,7 @@ void UASWaypointManager::writeWaypoints()
const MissionItem *cur_s = waypointsEditable.at(i);
cur_d->autocontinue = cur_s->getAutoContinue();
cur_d->current = cur_s->getCurrent() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen
cur_d->current = cur_s->isCurrentItem() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen
cur_d->param1 = cur_s->getParam1();
cur_d->param2 = cur_s->getParam2();
cur_d->param3 = cur_s->getParam3();
......@@ -1009,11 +1001,11 @@ void UASWaypointManager::writeWaypoints()
cur_d->frame = cur_s->getFrame();
cur_d->command = cur_s->getAction();
cur_d->seq = i; // don't read out the sequence number of the waypoint class
cur_d->x = cur_s->getX();
cur_d->y = cur_s->getY();
cur_d->z = cur_s->getZ();
cur_d->x = cur_s->x();
cur_d->y = cur_s->y();
cur_d->z = cur_s->z();
if (cur_s->getCurrent() && noCurrent)
if (cur_s->isCurrentItem() && noCurrent)
noCurrent = false;
if (i == (current_count - 1) && noCurrent == true) //not a single waypoint was set as "current"
cur_d->current = true; // set the last waypoint as current. Or should it better be the first waypoint ?
......@@ -1162,7 +1154,7 @@ UAS* UASWaypointManager::getUAS() {
float UASWaypointManager::getAltitudeRecommendation()
{
if (waypointsEditable.count() > 0) {
return waypointsEditable.last()->getAltitude();
return waypointsEditable.last()->altitude();
} else {
return HomePositionManager::instance()->getHomeAltitude() + getHomeAltitudeOffsetDefault();
}
......
......@@ -1313,7 +1313,7 @@ void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float widt
float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
float acceptRadius = w->getAcceptanceRadius();
double yawDiff = w->getYaw()/180.0*M_PI-yaw;
double yawDiff = w->yaw()/180.0*M_PI-yaw;
// Draw background
pen.setColor(Qt::black);
......@@ -1356,11 +1356,11 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
int frameRef = w->getFrame();
if (frameRef == MAV_FRAME_LOCAL_NED)
{
in = QPointF(w->getX(), w->getY());
in = QPointF(w->x(), w->y());
}
else if (frameRef == MAV_FRAME_LOCAL_ENU)
{
in = QPointF(w->getY(), w->getX());
in = QPointF(w->y(), w->x());
}
// Convert global coordinates into the local ENU frame, then display them.
else if (frameRef == MAV_FRAME_GLOBAL || frameRef == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
......@@ -1368,7 +1368,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
// Transform the lat/lon for this waypoint into the local frame
double e, n, u;
HomePositionManager::instance()->wgs84ToEnu(w->getX(), w->getY(),w->getZ(), &e, &n, &u);
HomePositionManager::instance()->wgs84ToEnu(w->x(), w->y(),w->z(), &e, &n, &u);
in = QPointF(n, e);
}
// Otherwise we don't process this waypoint.
......@@ -1383,7 +1383,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
QPointF p = metricBodyToRef(in);
// Select color based on if this is the current waypoint.
if (w->getCurrent())
if (w->isCurrentItem())
{
drawWaypoint(painter, QGC::colorYellow, refLineWidthToPen(0.8f), w, p);
}
......
......@@ -72,6 +72,7 @@ This file is part of the QGROUNDCONTROL project
#include "MultiVehicleManager.h"
#include "CustomCommandWidget.h"
#include "HomePositionManager.h"
#include "MissionEditor.h"
#ifdef UNITTEST_BUILD
#include "QmlControls/QmlTestWidget.h"
......@@ -425,6 +426,14 @@ void MainWindow::_buildPlanView(void)
}
}
void MainWindow::_buildMissionEditorView(void)
{
if (!_missionEditorView) {
_missionEditorView = new MissionEditor(this);
_missionEditorView->setVisible(false);
}
}
void MainWindow::_buildFlightView(void)
{
if (!_flightView) {
......@@ -700,6 +709,7 @@ void MainWindow::connectCommonActions()
perspectives->addAction(_ui.actionFlight);
perspectives->addAction(_ui.actionSimulationView);
perspectives->addAction(_ui.actionPlan);
perspectives->addAction(_ui.actionMissionEditor);
perspectives->addAction(_ui.actionSetup);
perspectives->setExclusive(true);
......@@ -724,6 +734,11 @@ void MainWindow::connectCommonActions()
_ui.actionPlan->setChecked(true);
_ui.actionPlan->activate(QAction::Trigger);
}
if (_currentView == VIEW_MISSIONEDITOR)
{
_ui.actionMissionEditor->setChecked(true);
_ui.actionMissionEditor->activate(QAction::Trigger);
}
if (_currentView == VIEW_SETUP)
{
_ui.actionSetup->setChecked(true);
......@@ -738,11 +753,12 @@ void MainWindow::connectCommonActions()
connect(MultiVehicleManager::instance(), &MultiVehicleManager::vehicleRemoved, this, &MainWindow::_vehicleRemoved);
// Views actions
connect(_ui.actionFlight, SIGNAL(triggered()), this, SLOT(loadFlightView()));
connect(_ui.actionSimulationView, SIGNAL(triggered()), this, SLOT(loadSimulationView()));
connect(_ui.actionAnalyze, SIGNAL(triggered()), this, SLOT(loadAnalyzeView()));
connect(_ui.actionPlan, SIGNAL(triggered()), this, SLOT(loadPlanView()));
connect(_ui.actionFlight, SIGNAL(triggered()), this, SLOT(loadFlightView()));
connect(_ui.actionSimulationView, SIGNAL(triggered()), this, SLOT(loadSimulationView()));
connect(_ui.actionAnalyze, SIGNAL(triggered()), this, SLOT(loadAnalyzeView()));
connect(_ui.actionPlan, SIGNAL(triggered()), this, SLOT(loadPlanView()));
connect(_ui.actionMissionEditor, SIGNAL(triggered()), this, SLOT(loadMissionEditorView()));
// Help Actions
connect(_ui.actionOnline_Documentation, SIGNAL(triggered()), this, SLOT(showHelp()));
connect(_ui.actionDeveloper_Credits, SIGNAL(triggered()), this, SLOT(showCredits()));
......@@ -891,6 +907,11 @@ void MainWindow::_loadCurrentViewState(void)
defaultWidgets = "WAYPOINT_LIST_DOCKWIDGET";
break;
case VIEW_MISSIONEDITOR:
_buildMissionEditorView();
centerView = _missionEditorView;
break;
case VIEW_SIMULATION:
_buildSimView();
centerView = _simView;
......@@ -993,6 +1014,17 @@ void MainWindow::loadPlanView()
}
}
void MainWindow::loadMissionEditorView()
{
if (_currentView != VIEW_MISSIONEDITOR)
{
_storeCurrentViewState();
_currentView = VIEW_MISSIONEDITOR;
_ui.actionMissionEditor->setChecked(true);
_loadCurrentViewState();
}
}
void MainWindow::loadSetupView()
{
if (_currentView != VIEW_SETUP)
......
......@@ -130,17 +130,13 @@ public slots:
/** @brief Show the application settings */
void showSettings();
/** @brief Load configuration views */
void loadSetupView();
/** @brief Load view for pilot */
void loadFlightView();
/** @brief Load view for simulation */
void loadSimulationView();
/** @brief Load view for engineer */
void loadAnalyzeView();
/** @brief Load New (QtQuick) Map View (Mission) */
void loadPlanView();
/** @brief Manage Links */
void loadMissionEditorView();
void manageLinks();
/** @brief Show the online help for users */
......@@ -204,12 +200,13 @@ protected:
typedef enum _VIEW_SECTIONS
{
VIEW_ANALYZE, // Engineering/Analyze view mode. Used for analyzing data and modifying onboard parameters
VIEW_PLAN, // New (QtQuick) Mission/Map/Plan view mode. Used for setting mission waypoints and high-level system commands.
VIEW_PLAN, // Old mission editor
VIEW_FLIGHT, // Flight/Fly/Operate view mode. Used for 1st-person observation of the vehicle.
VIEW_SIMULATION, // HIL Simulation view. Useful overview of the entire system when doing hardware-in-the-loop simulations.
VIEW_SETUP, // Setup view. Used for initializing the system for operation. Includes UI for calibration, firmware updating/checking, and parameter modifcation.
VIEW_UNUSED1, // Unused (don't remove, or it will screw up saved settigns indices)
VIEW_UNUSED2, // Unused (don't remove, or it will screw up saved settigns indices)
VIEW_MISSIONEDITOR, // New mission editor
} VIEW_SECTIONS;
/** @brief Catch window resize events */
......@@ -288,6 +285,7 @@ private:
QPointer<QWidget> _analyzeView;
QPointer<QWidget> _simView;
QPointer<QWidget> _terminalView;
QPointer<QWidget> _missionEditorView;
// Dock widget names
static const char* _uasControlDockWidgetName;
......@@ -316,6 +314,7 @@ private:
void _buildAnalyzeView(void);
void _buildSimView(void);
void _buildTerminalView(void);
void _buildMissionEditorView(void);
void _storeCurrentViewState(void);
void _loadCurrentViewState(void);
......@@ -344,7 +343,6 @@ private:
QString _getWindowStateKey();
QString _getWindowGeometryKey();
};
#endif /* _MAINWINDOW_H_ */
Supports Markdown
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