Commit d0d6cc42 authored by Don Gagne's avatar Don Gagne

More MissionEditor work

- New MissionManager
- Plumb MissionManager through MissionEditor
- Mission Editor supports get/set
parent 830d7dee
......@@ -145,6 +145,7 @@ INCLUDEPATH += \
src/Joystick \
src/lib/qmapcontrol \
src/MissionEditor \
src/MissionManager \
src/QmlControls \
src/uas \
src/ui \
......@@ -246,6 +247,7 @@ HEADERS += \
src/LogCompressor.h \
src/MG.h \
src/MissionEditor/MissionEditor.h \
src/MissionManager/MissionManager.h \
src/QGC.h \
src/QGCApplication.h \
src/QGCComboBox.h \
......@@ -384,6 +386,7 @@ SOURCES += \
src/LogCompressor.cc \
src/main.cc \
src/MissionEditor/MissionEditor.cc \
src/MissionManager/MissionManager.cc \
src/QGC.cc \
src/QGCApplication.cc \
src/QGCComboBox.cc \
......
......@@ -34,7 +34,7 @@ import QGroundControl.Controllers 1.0
import QGroundControl.ScreenTools 1.0
QGCView {
id: rootQGCView
id: qgcView
viewPanel: panel
QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled }
......
......@@ -53,7 +53,6 @@ Map {
property real lon: (longitude >= -180 && longitude <= 180) ? longitude : 0
property real lat: (latitude >= -90 && latitude <= 90) ? latitude : 0
anchors.fill: parent
zoomLevel: 18
center: QtPositioning.coordinate(lat, lon)
gesture.flickDeceleration: 3000
......
......@@ -23,6 +23,8 @@ This file is part of the QGROUNDCONTROL project
#include "MissionEditor.h"
#include "ScreenToolsController.h"
#include "MultiVehicleManager.h"
#include "MissionManager.h"
#include <QQmlContext>
#include <QQmlEngine>
......@@ -32,6 +34,7 @@ const char* MissionEditor::_settingsGroup = "MissionEditor";
MissionEditor::MissionEditor(QWidget *parent)
: QGCQmlWidgetHolder(parent)
, _missionItems(NULL)
{
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
// Get rid of layout default margins
......@@ -44,6 +47,15 @@ MissionEditor::MissionEditor(QWidget *parent)
setMinimumHeight(33 * ScreenToolsController::defaultFontPixelSize_s());
#endif
Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
if (activeVehicle) {
MissionManager* missionManager = activeVehicle->missionManager();
connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditor::_newMissionItemsAvailable);
_newMissionItemsAvailable();
} else {
_missionItems = new QmlObjectListModel(this);
}
setContextPropertyObject("controller", this);
setSource(QUrl::fromUserInput("qrc:/qml/MissionEditor.qml"));
......@@ -53,6 +65,34 @@ MissionEditor::~MissionEditor()
{
}
void MissionEditor::_newMissionItemsAvailable(void)
{
if (_missionItems) {
_missionItems->deleteLater();
}
_missionItems = MultiVehicleManager::instance()->activeVehicle()->missionManager()->copyMissionItems();
emit missionItemsChanged();
}
void MissionEditor::getMissionItems(void)
{
Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
if (activeVehicle) {
activeVehicle->missionManager()->requestMissionItems();
}
}
void MissionEditor::setMissionItems(void)
{
Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
if (activeVehicle) {
activeVehicle->missionManager()->writeMissionItems(*_missionItems);
}
}
void MissionEditor::saveSetting(const QString &name, const QString& value)
{
QSettings settings;
......@@ -73,10 +113,10 @@ QString MissionEditor::loadSetting(const QString &name, const QString& defaultVa
void MissionEditor::addMissionItem(QGeoCoordinate coordinate)
{
MissionItem * newItem = new MissionItem(this, _missionItems.count(), coordinate);
if (_missionItems.count() == 0) {
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);
_missionItems->append(newItem);
}
......@@ -35,19 +35,27 @@ public:
MissionEditor(QWidget* parent = NULL);
~MissionEditor();
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel CONSTANT)
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel NOTIFY missionItemsChanged)
Q_INVOKABLE void addMissionItem(QGeoCoordinate coordinate);
Q_INVOKABLE void getMissionItems(void);
Q_INVOKABLE void setMissionItems(void);
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; }
QmlObjectListModel* missionItemsModel(void) { return _missionItems; }
signals:
void missionItemsChanged(void);
private slots:
void _newMissionItemsAvailable();
private:
QmlObjectListModel _missionItems;
QmlObjectListModel* _missionItems;
static const char* _settingsGroup;
};
......
......@@ -34,82 +34,147 @@ 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.
QGCView {
viewPanel: panel
readonly property real _defaultLatitude: 37.803784
readonly property real _defaultLongitude: -122.462276
readonly property int _decimalPlaces: 7
readonly property real _horizontalMargin: ScreenTools.defaultFontPixelWidth / 2
readonly property real _verticalMargin: ScreenTools.defaultFontPixelHeight / 2
readonly property var _activeVehicle: multiVehicleManager.activeVehicle
FlightMap {
id: editorMap
anchors.fill: parent
mapName: "MissionEditor"
latitude: _defaultLatitude
longitude: _defaultLongitude
QGCLabel { text: "WIP: Non functional"; font.pixelSize: ScreenTools.largeFontPixelSize }
QGCPalette { id: _qgcPal; colorGroupEnabled: enabled }
QGCViewPanel {
id: panel
anchors.fill: parent
MouseArea {
Item {
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)
FlightMap {
id: editorMap
anchors.left: parent.left
anchors.right: missionItemView.left
anchors.top: parent.top
anchors.bottom: parent.bottom
mapName: "MissionEditor"
latitude: _defaultLatitude
longitude: _defaultLongitude
QGCLabel {
anchors.right: parent.right
text: "WIP: Danger, do not fly with this!"; 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)
}
}
}
// 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
// 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)
}
}
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
}
}
}
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
}
}
}
}
} // FlightMap
Rectangle {
id: missionItemView
anchors.right: parent.right
anchors.top: parent.top
anchors.bottom: parent.bottom
width: ScreenTools.defaultFontPixelWidth * 30
color: _qgcPal.window
Item {
anchors.margins: _verticalMargin
anchors.fill: parent
QGCButton {
id: toolsButton
text: "Tools"
menu : toolMenu
Menu {
id: toolMenu
MenuItem {
text: "Get mission items from vehicle"
enabled: _activeVehicle && !_activeVehicle.missionManager.inProgress
onTriggered: controller.getMissionItems()
}
MenuItem {
text: "Send mission items to vehicle"
enabled: _activeVehicle && !_activeVehicle.missionManager.inProgress
onTriggered: controller.setMissionItems()
}
MenuItem {
text: "Load mission items from disk"
}
MenuItem {
text: "Save mission items to disk"
}
}
}
// Mission item list
ListView {
id: missionItemSummaryList
anchors.topMargin: _verticalMargin
anchors.left: parent.left
anchors.right: parent.right
anchors.top: toolsButton.bottom
anchors.bottom: parent.bottom
spacing: _verticalMargin
orientation: ListView.Vertical
model: controller.missionItems
property real _maxItemHeight: 0
delegate:
MissionItemEditor {
missionItem: object
width: parent.width
}
} // ListView
} // Item
} // Rectangle - mission item list
} // Item - split view container
} // QGCViewPanel
} // QGCVIew
This diff is collapsed.
......@@ -69,6 +69,7 @@ public:
Q_PROPERTY(QStringList valueStrings READ valueStrings NOTIFY valueStringsChanged)
Q_PROPERTY(int commandByIndex READ commandByIndex WRITE setCommandByIndex NOTIFY commandChanged)
Q_PROPERTY(QmlObjectListModel* facts READ facts NOTIFY commandChanged)
Q_PROPERTY(int factCount READ factCount NOTIFY commandChanged)
Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
// Property accesors
......@@ -90,13 +91,14 @@ public:
int commandByIndex(void);
void setCommandByIndex(int index);
MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)getAction(); };
MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)_command; };
void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) { setAction(command); }
QStringList valueLabels(void);
QStringList valueStrings(void);
QmlObjectListModel* facts(void);
int factCount(void);
double yawDegrees(void) const;
void setYawDegrees(double yaw);
......@@ -122,57 +124,56 @@ public:
double yawRadians(void) const;
void setYawRadians(double yaw);
bool getAutoContinue() const {
bool autoContinue() const {
return _autocontinue;
}
double loiterOrbitRadius() const {
return _loiterOrbitRadiusFact->value().toDouble();
}
double getAcceptanceRadius() const {
return getParam2();
double acceptanceRadius() const {
return param2();
}
double getHoldTime() const {
return getParam1();
double holdTime() const {
return param1();
}
double getParam1() const {
double param1() const {
return _param1Fact->value().toDouble();
}
double getParam2() const {
double param2() const {
return _param2Fact->value().toDouble();
}
double getParam3() const {
double param3() const {
return loiterOrbitRadius();
}
double getParam4() const {
double param4() const {
return yawRadians();
}
double getParam5() const {
double param5() const {
return latitude();
}
double getParam6() const {
double param6() const {
return longitude();
}
double getParam7() const {
double param7() const {
return altitude();
}
// MAV_FRAME
int getFrame() const {
int frame() const {
return _frame;
}
// MAV_CMD
int getAction() const {
return _action;
int command() const {
return _command;
}
/** @brief Returns true if x, y, z contain reasonable navigation data */
bool isNavigationType();
/** @brief Get the time this waypoint was reached */
quint64 getReachedTime() const { return _reachedTime; }
quint64 reachedTime() const { return _reachedTime; }
void save(QTextStream &saveStream);
bool load(QTextStream &loadStream);
signals:
void sequenceNumberChanged(int sequenceNumber);
void specifiesCoordinateChanged(bool specifiesCoordinate);
......@@ -224,13 +225,13 @@ private:
const char* name;
} MavCmd2Name_t;
int _sequenceNumber;
QGeoCoordinate _coordinate;
int _frame;
int _action;
bool _autocontinue;
bool _isCurrentItem;
quint64 _reachedTime;
int _sequenceNumber;
QGeoCoordinate _coordinate;
int _frame;
MavlinkQmlSingleton::Qml_MAV_CMD _command;
bool _autocontinue;
bool _isCurrentItem;
quint64 _reachedTime;
Fact* _yawRadiansFact;
Fact* _loiterOrbitRadiusFact;
......
This diff is collapsed.
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 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 MissionManager_H
#define MissionManager_H
#include <QObject>
#include <QLoggingCategory>
#include <QThread>
#include <QMutex>
#include <QTimer>
#include "QmlObjectListModel.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
class Vehicle;
Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog)
class MissionManager : public QThread
{
Q_OBJECT
public:
/// @param uas Uas which this set of facts is associated with
MissionManager(Vehicle* vehicle);
~MissionManager();
Q_PROPERTY(bool inProgress READ inProgress CONSTANT)
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems CONSTANT)
// Property accessors
bool inProgress(void) { return _retryAck != AckNone; }
QmlObjectListModel* missionItems(void) { return &_missionItems; }
void requestMissionItems(void);
/// Writes the specified set of mission items to the vehicle
void writeMissionItems(const QmlObjectListModel& missionItems);
/// Returns a copy of the current set of mission items. Caller is responsible for
/// freeing returned object.
QmlObjectListModel* copyMissionItems(void);
signals:
void newMissionItemsAvailable(void);
void _requestMissionItemsOnThread(void);
void _writeMissionItemsOnThread(void);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
void _ackTimeout(void);
private:
typedef enum {
AckNone, ///< State machine is idle
AckMissionCount, ///< MISSION_COUNT message expected
AckMissionItem, ///< MISSION_ITEM expected
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
} AckType_t;
void _startAckTimeout(AckType_t ack, const mavlink_message_t& message);
bool _stopAckTimeout(AckType_t expectedAck);
void _sendTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message);
void _handleMissionRequest(const mavlink_message_t& message);
void _handleMissionAck(const mavlink_message_t& message);
void _requestNextMissionItem(int sequenceNumber);
void _clearMissionItems(void);
void _requestMissionItems(void);
void _writeMissionItems(void);
// Overrides from QThread
void run(void);
private:
Vehicle* _vehicle;
int _cMissionItems; ///< Mission items on vehicle
QTimer* _ackTimeoutTimer;
AckType_t _retryAck;
mavlink_message_t _retryMessage;
int _retryCount;
int _expectedSequenceNumber;
QMutex _dataMutex;
QmlObjectListModel _missionItems;
static const int _ackTimeoutMilliseconds= 1000;
static const int _maxRetryCount = 5;
};
#endif
\ No newline at end of file
......@@ -79,6 +79,7 @@
#include "MavlinkQmlSingleton.h"
#include "JoystickManager.h"
#include "QmlObjectListModel.h"
#include "MissionManager.h"
#ifndef __ios__
#include "SerialLink.h"
......@@ -294,6 +295,7 @@ void QGCApplication::_initCommon(void)
qmlRegisterUncreatableType<VehicleComponent> ("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Can only reference, cannot create");
qmlRegisterUncreatableType<Vehicle> ("QGroundControl.Vehicle", 1, 0, "Vehicle", "Can only reference, cannot create");
qmlRegisterUncreatableType<MissionItem> ("QGroundControl.Vehicle", 1, 0, "MissionItem", "Can only reference, cannot create");
qmlRegisterUncreatableType<MissionManager> ("QGroundControl.Vehicle", 1, 0, "MissionManager", "Can only reference, cannot create");
qmlRegisterUncreatableType<JoystickManager> ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only");
qmlRegisterUncreatableType<Joystick> ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only");
qmlRegisterUncreatableType<QmlObjectListModel> ("QGroundControl", 1, 0, "QmlObjectListModel", "Reference only");
......
This diff is collapsed.
......@@ -112,7 +112,12 @@ bool QmlObjectListModel::removeRows(int position, int rows, const QModelIndex& p
return true;
}
QObject*& QmlObjectListModel::operator[](int index)
QObject* QmlObjectListModel::operator[](int index)
{
return _objectList[index];
}
const QObject* QmlObjectListModel::operator[](int index) const
{
return _objectList[index];
}
......@@ -135,7 +140,7 @@ void QmlObjectListModel::append(QObject* object)
insertRows(_objectList.count() - 1, 1);
}
int QmlObjectListModel::count(void)
int QmlObjectListModel::count(void) const
{
return rowCount();
}
......
......@@ -38,11 +38,12 @@ public:
Q_PROPERTY(int count READ count NOTIFY countChanged)
int count(void);
int count(void) const;
void append(QObject* object);
void clear(void);
void removeAt(int i);
QObject*& operator[](int i);
QObject* operator[](int i);
const QObject* operator[](int i) const;
// Overrides from QAbstractListModel
virtual int rowCount(const QModelIndex & parent = QModelIndex()) const;
......
......@@ -30,6 +30,7 @@
#include "UASMessageHandler.h"
#include "UAS.h"
#include "JoystickManager.h"
#include "MissionManager.h"
QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")
......@@ -151,6 +152,10 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
_waypointViewOnlyListChanged();
_loadSettings();
if (qgcApp()->useNewMissionEditor()) {
_missionManager = new MissionManager(this);
}
}
Vehicle::~Vehicle()
......@@ -187,7 +192,7 @@ Vehicle::~Vehicle()
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
if (message.sysid != _id) {
if (message.sysid != _id && message.sysid != 0) {
return;
}
......@@ -195,6 +200,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_addLink(link);
}
emit mavlinkMessageReceived(message);
_uas->receiveMessage(message);
}
......
......@@ -41,6 +41,7 @@ class UASInterface;
class FirmwarePlugin;
class AutoPilotPlugin;
class UASWaypointManager;
class MissionManager;
Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
......@@ -55,7 +56,8 @@ public:
Q_PROPERTY(int id READ id CONSTANT)
Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT)
Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged)
Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged)
Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT)
Q_INVOKABLE QString getMavIconColor();
......@@ -147,6 +149,8 @@ public:
int manualControlReservedButtonCount(void);
MissionManager* missionManager(void) { return _missionManager; }