Commit 02dd86e5 authored by DonLakeFlyer's avatar DonLakeFlyer

More Plan re-architecture

This time to handle correctly updating plan/fly view when plan changes
parent 2b712a47
......@@ -82,6 +82,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlOb
// Total point count, +1 polygon close in last index, +1 for breach in index 0
_cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints;
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
// FIXME: No validation of correct fence received
......@@ -89,7 +90,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlOb
_sendFencePoint(index);
}
emit loadComplete(_breachReturnPoint, _polygon);
emit sendComplete();
}
void APMGeoFenceManager::loadFromVehicle(void)
......@@ -326,7 +327,10 @@ void APMGeoFenceManager::_parametersReady(void)
void APMGeoFenceManager::removeAll(void)
{
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll";
QmlObjectListModel emptyPolygon;
sendToVehicle(_breachReturnPoint, emptyPolygon);
emit removeAllComplete();
}
......@@ -48,7 +48,7 @@ void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
_sendRallyPoint(index);
}
emit loadComplete(_rgPoints);
emit sendComplete();
}
void APMRallyPointManager::loadFromVehicle(void)
......@@ -60,7 +60,7 @@ void APMRallyPointManager::loadFromVehicle(void)
_rgPoints.clear();
_cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt();
qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << _cReadRallyPoints;
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints;
if (_cReadRallyPoints == 0) {
emit loadComplete(_rgPoints);
return;
......@@ -152,7 +152,10 @@ bool APMRallyPointManager::rallyPointsSupported(void) const
void APMRallyPointManager::removeAll(void)
{
qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll";
QList<QGeoCoordinate> noRallyPoints;
sendToVehicle(noRallyPoints);
emit removeAllComplete();
}
......@@ -20,6 +20,7 @@
#include "JsonHelper.h"
#include "QGCQGeoCoordinate.h"
#include "AppSettings.h"
#include "PlanMasterController.h"
#ifndef __mobile__
#include "MainWindow.h"
......@@ -39,6 +40,7 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q
, _geoFenceManager(_managerVehicle->geoFenceManager())
, _dirty(false)
, _mapPolygon(this)
, _itemsRequested(false)
{
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
......@@ -104,7 +106,9 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged);
connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_loadComplete);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete);
connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete);
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
_signalAll();
......@@ -152,20 +156,40 @@ void GeoFenceController::removeAll(void)
_mapPolygon.clear();
}
void GeoFenceController::removeAllFromVehicle(void)
{
if (_masterController->offline()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::removeAllFromVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::removeAllFromVehicle called while syncInProgress";
} else {
_geoFenceManager->removeAll();
}
}
void GeoFenceController::loadFromVehicle(void)
{
if (!syncInProgress()) {
_geoFenceManager->loadFromVehicle();
if (_masterController->offline()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle called while syncInProgress";
} else {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call while syncInProgress";
_itemsRequested = true;
_geoFenceManager->loadFromVehicle();
}
}
void GeoFenceController::sendToVehicle(void)
{
_geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
_mapPolygon.setDirty(false);
setDirty(false);
if (_masterController->offline()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
} else {
_geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
_mapPolygon.setDirty(false);
setDirty(false);
}
}
bool GeoFenceController::syncInProgress(void) const
......@@ -252,12 +276,32 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
emit breachReturnPointChanged(_breachReturnPoint);
}
void GeoFenceController::_loadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
{
_setReturnPointFromManager(breachReturn);
_setPolygonFromManager(polygon);
setDirty(false);
emit loadComplete();
// Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) {
_setReturnPointFromManager(breachReturn);
_setPolygonFromManager(polygon);
setDirty(false);
_signalAll();
emit loadComplete();
}
_itemsRequested = false;
}
void GeoFenceController::_managerSendComplete(void)
{
// Fly view always reloads on manager sendComplete
if (!_editMode) {
showPlanFromManagerVehicle();
}
}
void GeoFenceController::_managerRemoveAllComplete(void)
{
// Remove all from vehicle so we always update
showPlanFromManagerVehicle();
}
bool GeoFenceController::containsItems(void) const
......@@ -270,7 +314,28 @@ void GeoFenceController::_updateContainsItems(void)
emit containsItemsChanged(containsItems());
}
void GeoFenceController::removeAllFromVehicle(void)
bool GeoFenceController::showPlanFromManagerVehicle(void)
{
_geoFenceManager->removeAll();
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle";
if (_masterController->offline()) {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::showPlanFromManagerVehicle called while offline";
return true; // stops further propogation of showPlanFromManagerVehicle due to error
} else {
_itemsRequested = true;
if (!_managerVehicle->initialPlanRequestComplete()) {
// The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
return true;
} else if (syncInProgress()) {
// If the sync is already in progress, _loadComplete will be called automatically when it is done. So no need to do anything.
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal";
return true;
} else {
// Fake a _loadComplete with the current items
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true;
_managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon());
return false;
}
}
}
......@@ -56,6 +56,7 @@ public:
void setDirty (bool dirty) final;
bool containsItems (void) const final;
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
bool circleEnabled (void) const;
Fact* circleRadiusFact (void) const;
......@@ -87,8 +88,10 @@ private slots:
void _setDirty(void);
void _setPolygonFromManager(const QList<QGeoCoordinate>& polygon);
void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
void _loadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
void _updateContainsItems(void);
void _managerSendComplete(void);
void _managerRemoveAllComplete(void);
private:
void _init(void);
......@@ -98,6 +101,7 @@ private:
bool _dirty;
QGCMapPolygon _mapPolygon;
QGeoCoordinate _breachReturnPoint;
bool _itemsRequested;
static const char* _jsonFileTypeValue;
static const char* _jsonBreachReturnKey;
......
......@@ -43,5 +43,12 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjec
// No geofence support in unknown vehicle
Q_UNUSED(breachReturn);
Q_UNUSED(polygon);
emit loadComplete(QGeoCoordinate(), QList<QGeoCoordinate>());
emit sendComplete();
}
void GeoFenceManager::removeAll(void)
{
// No geofence support in unknown vehicle
emit removeAllComplete();
}
......@@ -35,13 +35,16 @@ public:
virtual bool inProgress(void) const { return false; }
/// Load the current settings from the vehicle
/// Signals loadComplete when done
virtual void loadFromVehicle(void);
/// Send the current settings to the vehicle
/// Signals sendComplete when done
virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon);
/// Remove all fence related items from vehicle (does not affect paramters)
virtual void removeAll(void) { }
/// Signals removeAllComplete when done
virtual void removeAll(void);
/// Returns true if this vehicle support polygon fence
/// Signal: polygonSupportedChanged
......@@ -93,6 +96,8 @@ signals:
void polygonSupportedChanged (bool polygonSupported);
void polygonEnabledChanged (bool polygonEnabled);
void breachReturnSupportedChanged (bool breachReturnSupported);
void removeAllComplete (void);
void sendComplete (void);
protected:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
......
This diff is collapsed.
......@@ -123,6 +123,7 @@ public:
void setDirty (bool dirty) final;
bool containsItems (void) const final;
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
// Property accessors
......@@ -178,6 +179,8 @@ private slots:
void _cameraFeedback(QGeoCoordinate imageCoordinate, int index);
void _progressPctChanged(double progressPct);
void _visualItemsDirtyChanged(bool dirty);
void _managerSendComplete(void);
void _managerRemoveAllComplete(void);
private:
void _init(void);
......@@ -194,7 +197,7 @@ private:
bool _findPreviousAltitude(int newIndex, double* prevAltitude, MAV_FRAME* prevFrame);
static double _normalizeLat(double lat);
static double _normalizeLon(double lon);
static void _addMissionSettings(Vehicle* vehicle, QmlObjectListModel* visualItems, bool addToCenter);
void _addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter);
bool _loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString);
bool _loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
......@@ -218,7 +221,7 @@ private:
QmlObjectListModel _cameraPoints;
CoordVectHashTable _linesTable;
bool _firstItemsFromVehicle;
bool _missionItemsRequested;
bool _itemsRequested;
MissionFlightStatus_t _missionFlightStatus;
QString _surveyMissionItemName;
QString _fwLandingMissionItemName;
......
......@@ -168,16 +168,16 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
emit inProgressChanged(true);
}
void MissionManager::requestMissionItems(void)
void MissionManager::loadFromVehicle(void)
{
if (_vehicle->isOfflineEditingVehicle()) {
return;
}
qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
qCDebug(MissionManagerLog) << "loadFromVehicle read sequence";
if (inProgress()) {
qCDebug(MissionManagerLog) << "requestMissionItems called while transaction in progress";
qCDebug(MissionManagerLog) << "loadFromVehicle called while transaction in progress";
return;
}
......@@ -339,7 +339,6 @@ void MissionManager::_readTransactionComplete(void)
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_finishTransaction(true);
emit newMissionItemsAvailable(false);
}
void MissionManager::_handleMissionCount(const mavlink_message_t& message)
......@@ -863,24 +862,35 @@ void MissionManager::_finishTransaction(bool success)
{
emit progressPct(1);
if (!success && _transactionInProgress == TransactionRead) {
// Read from vehicle failed, clear partial list
_clearAndDeleteMissionItems();
emit newMissionItemsAvailable(false);
}
if (_transactionInProgress == TransactionWrite) {
emit newMissionItemsAvailable(_missionItems.count() == 0);
}
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
// First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete.
TransactionType_t currentTransactionType = _transactionInProgress;
_transactionInProgress = TransactionNone;
if (_transactionInProgress != TransactionNone) {
_transactionInProgress = TransactionNone;
emit inProgressChanged(false);
}
switch (currentTransactionType) {
case TransactionRead:
if (!success) {
// Read from vehicle failed, clear partial list
_clearAndDeleteMissionItems();
}
emit newMissionItemsAvailable(false);
break;
case TransactionWrite:
emit sendComplete();
break;
case TransactionRemoveAll:
emit removeAllComplete();
break;
default:
break;
}
if (_resumeMission) {
_resumeMission = false;
emit resumeMissionReady();
......@@ -945,9 +955,8 @@ void MissionManager::removeAll(void)
_lastCurrentIndex = -1;
emit currentIndexChanged(-1);
emit lastCurrentIndexChanged(-1);
emit newMissionItemsAvailable(true /* removeAllRequested */);
_transactionInProgress = TransactionClearAll;
_transactionInProgress = TransactionRemoveAll;
_retryCount = 0;
emit inProgressChanged(true);
......
......@@ -43,10 +43,13 @@ public:
/// Last current mission item reported while in Mission flight mode
int lastCurrentIndex(void) const { return _lastCurrentIndex; }
void requestMissionItems(void);
/// Load the mission items from the vehicle
/// Signals newMissionItemsAvailable when done
void loadFromVehicle(void);
/// Writes the specified set of mission items to the vehicle
/// @param missionItems Items to send to vehicle
/// Signals sendComplete when done
void writeMissionItems(const QList<MissionItem*>& missionItems);
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
......@@ -55,6 +58,7 @@ public:
void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);
/// Removes all mission items from vehicle
/// Signals removeAllComplete when done
void removeAll(void);
/// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
......@@ -86,6 +90,8 @@ signals:
void resumeMissionReady(void);
void cameraFeedback(QGeoCoordinate imageCoordinate, int index);
void progressPct(double progressPercentPct);
void removeAllComplete (void);
void sendComplete (void);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
......@@ -105,7 +111,7 @@ private:
TransactionNone,
TransactionRead,
TransactionWrite,
TransactionClearAll
TransactionRemoveAll
} TransactionType_t;
void _startAckTimeout(AckType_t ack);
......
......@@ -120,7 +120,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode
_mockLink->setMissionItemFailureMode(failureMode);
// Read the items back from the vehicle
_missionManager->requestMissionItems();
_missionManager->loadFromVehicle();
// requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
......
......@@ -33,3 +33,8 @@ void PlanElementController::start(bool editMode)
{
_editMode = editMode;
}
void PlanElementController::managerVehicleChanged(Vehicle* managerVehicle)
{
_managerVehicle = managerVehicle;
}
......@@ -35,19 +35,26 @@ public:
/// @param editMode true: controller being used in Plan view, false: controller being used in Fly view
virtual void start(bool editMode);
virtual void save(QJsonObject& json) = 0;
virtual bool load(const QJsonObject& json, QString& errorString) = 0;
virtual void loadFromVehicle(void) = 0;
virtual void sendToVehicle(void) = 0;
virtual void removeAll(void) = 0; ///< Removes all from controller only, synce required to remove from vehicle
virtual void removeAllFromVehicle(void) = 0; ///< Removes all from vehicle and controller
virtual void save (QJsonObject& json) = 0;
virtual bool load (const QJsonObject& json, QString& errorString) = 0;
virtual void loadFromVehicle (void) = 0;
virtual void removeAll (void) = 0; ///< Removes all from controller only
virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete
virtual bool containsItems (void) const = 0;
virtual bool syncInProgress (void) const = 0;
virtual bool dirty (void) const = 0;
virtual void setDirty (bool dirty) = 0;
/// Called when a new manager vehicle has been set. Derived classes should override to implement custom behavior.
/// Sends the current plan element to the vehicle
/// Signals sendComplete when done
virtual void sendToVehicle(void) = 0;
/// Removes all from vehicle and controller
/// Signals removeAllComplete when done
virtual void removeAllFromVehicle(void) = 0;
/// Called when a new manager vehicle has been set.
virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0;
signals:
......@@ -55,6 +62,8 @@ signals:
void syncInProgressChanged (bool syncInProgress);
void dirtyChanged (bool dirty);
void vehicleChanged (Vehicle* vehicle);
void sendComplete (void);
void removeAllComplete (void);
protected:
PlanMasterController* _masterController;
......
......@@ -18,6 +18,8 @@
#include <QJsonDocument>
#include <QFileInfo>
QGC_LOGGING_CATEGORY(PlanMasterControllerLog, "PlanMasterControllerLog")
const int PlanMasterController::_planFileVersion = 1;
const char* PlanMasterController::_planFileType = "Plan";
const char* PlanMasterController::_jsonMissionObjectKey = "mission";
......@@ -38,6 +40,7 @@ PlanMasterController::PlanMasterController(QObject* parent)
, _loadRallyPoints(false)
, _sendGeoFence(false)
, _sendRallyPoints(false)
, _syncInProgress(false)
{
connect(&_missionController, &MissionController::dirtyChanged, this, &PlanMasterController::dirtyChanged);
connect(&_geoFenceController, &GeoFenceController::dirtyChanged, this, &PlanMasterController::dirtyChanged);
......@@ -81,6 +84,8 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
return;
}
qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged" << activeVehicle;
bool newOffline = false;
if (activeVehicle == NULL) {
// Since there is no longer an active vehicle we use the offline controller vehicle as the manager vehicle
......@@ -96,8 +101,12 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
appSettings->offlineEditingVehicleType()->setRawValue(AppSettings::offlineEditingVehicleTypeFromVehicleType(_managerVehicle->vehicleType()));
// We use these signals to sequence upload and download to the multiple controller/managers
connect(_managerVehicle->missionManager(), &MissionManager::newMissionItemsAvailable, this, &PlanMasterController::_loadSendMissionComplete);
connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::loadComplete, this, &PlanMasterController::_loadSendGeoFenceCompelte);
connect(_managerVehicle->missionManager(), &MissionManager::newMissionItemsAvailable, this, &PlanMasterController::_loadMissionComplete);
connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::loadComplete, this, &PlanMasterController::_loadGeoFenceComplete);
connect(_managerVehicle->rallyPointManager(), &RallyPointManager::loadComplete, this, &PlanMasterController::_loadRallyPointsComplete);
connect(_managerVehicle->missionManager(), &MissionManager::sendComplete, this, &PlanMasterController::_sendMissionComplete);
connect(_managerVehicle->geoFenceManager(), &GeoFenceManager::sendComplete, this, &PlanMasterController::_sendGeoFenceComplete);
connect(_managerVehicle->rallyPointManager(), &RallyPointManager::sendComplete, this, &PlanMasterController::_sendRallyPointsComplete);
}
if (newOffline != _offline) {
_offline = newOffline;
......@@ -108,35 +117,77 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
_geoFenceController.managerVehicleChanged(_managerVehicle);
_rallyPointController.managerVehicleChanged(_managerVehicle);
if (!_editMode && _offline) {
// Fly view has changed to a new active vehicle
loadFromVehicle();
if (_editMode) {
if (!offline()) {
// We are in Plan view and we have a newly connected vehicle:
// - If there is no plan available in Plan view show the one from the vehicle
// - Otherwise leave the current plan alone
if (!containsItems()) {
qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Plan view is empty so loading from manager";
_showPlanFromManagerVehicle();
}
}
} else {
if (offline()) {
// No more active vehicle, clear mission
qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is offline clearing plan";
removeAll();
} else {
// Fly view has changed to a new active vehicle, update to show correct mission
qCDebug(PlanMasterControllerLog) << "_activeVehicleChanged: Fly view is online so loading from manager";
_showPlanFromManagerVehicle();
}
}
// Whenever manager changes we need to update syncInProgress
emit syncInProgressChanged(syncInProgress());
}
void PlanMasterController::loadFromVehicle(void)
{
if (!offline()) {
if (offline()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline";
} else if (!_editMode) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called from Fly view";
} else if (syncInProgress()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while syncInProgress";
} else {
_loadGeoFence = true;
_syncInProgress = true;
_missionController.loadFromVehicle();
setDirty(false);
} else {
qWarning() << "PlanMasterController::sendToVehicle called while offline";
}
}
void PlanMasterController::_loadSendMissionComplete(void)
void PlanMasterController::_loadMissionComplete(void)
{
if (_loadGeoFence) {
if (_editMode && _loadGeoFence) {
_loadGeoFence = false;
_loadRallyPoints = true;
_geoFenceController.loadFromVehicle();
setDirty(false);
} else if (_sendGeoFence) {
}
}
void PlanMasterController::_loadGeoFenceComplete(void)
{
if (_editMode && _loadRallyPoints) {
_loadRallyPoints = false;
_rallyPointController.loadFromVehicle();
setDirty(false);
}
}
void PlanMasterController::_loadRallyPointsComplete(void)
{
if (_editMode) {
_syncInProgress = false;
emit syncInProgressChanged(false);
}
}
void PlanMasterController::_sendMissionComplete(void)
{
if (_editMode && _sendGeoFence) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle";
_sendGeoFence = false;
_sendRallyPoints = true;
_geoFenceController.sendToVehicle();
......@@ -144,26 +195,37 @@ void PlanMasterController::_loadSendMissionComplete(void)
}
}
void PlanMasterController::_loadSendGeoFenceCompelte(void)
void PlanMasterController::_sendGeoFenceComplete(void)
{
if (_loadRallyPoints) {
_loadRallyPoints = false;
_rallyPointController.loadFromVehicle();
setDirty(false);
} else if (_sendRallyPoints) {
if (_editMode && _sendRallyPoints) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
_sendRallyPoints = false;
_rallyPointController.sendToVehicle();
}
}
void PlanMasterController::_sendRallyPointsComplete(void)
{
if (_editMode && _syncInProgress) {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete";
_syncInProgress = false;
emit syncInProgressChanged(false);
}
}
void PlanMasterController::sendToVehicle(void)
{
if (!offline()) {
if (offline()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline";
} else if (!_editMode) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called from Fly view";
} else if (syncInProgress()) {
qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while syncInProgress";
} else {
qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start mission sendToVehicle";
_sendGeoFence = true;
_missionController.sendToVehicle();
setDirty(false);
} else {
qWarning() << "PlanMasterController::sendToVehicle called while offline";
}
}
......@@ -293,11 +355,6 @@ bool PlanMasterController::containsItems(void) const
return _missionController.containsItems() || _geoFenceController.containsItems() || _rallyPointController.containsItems();
}
bool PlanMasterController::syncInProgress(void) const
{
return _missionController.syncInProgress() || _geoFenceController.syncInProgress() || _rallyPointController.syncInProgress();
}
bool PlanMasterController::dirty(void) const
{
return _missionController.dirty() || _geoFenceController.dirty() || _rallyPointController.dirty();
......@@ -341,3 +398,13 @@ void PlanMasterController::sendPlanToVehicle(Vehicle* vehicle, const QString& fi
controller->loadFromFile(filename);
delete controller;
}
void PlanMasterController::_showPlanFromManagerVehicle(void)
{
// The crazy if structure is to handle the load propogating by itself through the system
if (!_missionController.showPlanFromManagerVehicle()) {
if (!_geoFenceController.showPlanFromManagerVehicle()) {
_rallyPointController.showPlanFromManagerVehicle();
}
}
}
......@@ -16,6 +16,9 @@
#include "RallyPointController.h"
#include "Vehicle.h"
#include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h"
Q_DECLARE_LOGGING_CATEGORY(PlanMasterControllerLog)
/// Master controller for mission, fence, rally
class PlanMasterController : public QObject
......@@ -64,7 +67,7 @@ public:
bool offline (void) const { return _offline; }
bool containsItems (void) const;
bool syncInProgress (void) const;
bool syncInProgress (void) const { return _syncInProgress; }
bool dirty (void) const;
void setDirty (bool dirty);
QString fileExtension (void) const;
......@@ -83,10 +86,16 @@ signals:
private slots:
void _activeVehicleChanged(Vehicle* activeVehicle);
void _loadSendMissionComplete(void);
void _loadSendGeoFenceCompelte(void);
void _loadMissionComplete(void);
void _loadGeoFenceComplete(void);
void _loadRallyPointsComplete(void);
void _sendMissionComplete(void);
void _sendGeoFenceComplete(void);
void _sendRallyPointsComplete(void);
private:
void _showPlanFromManagerVehicle(void);
MultiVehicleManager* _multiVehicleMgr;
Vehicle* _controllerVehicle;
Vehicle* _managerVehicle;
......@@ -99,6 +108,7 @@ private:
bool _loadRallyPoints;
bool _sendGeoFence;
bool _sendRallyPoints;
bool _syncInProgress;
static const int _planFileVersion;
static const char* _planFileType;
......
......@@ -23,6 +23,7 @@
#include "QGroundControlQmlGlobal.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#include "PlanMasterController.h"
#ifndef __mobile__
#include "QGCQFileDialog.h"
......@@ -41,6 +42,7 @@ RallyPointController::RallyPointController(PlanMasterController* masterControlle
, _rallyPointManager(_managerVehicle->rallyPointManager())
, _dirty(false)
, _currentRallyPoint(NULL)
, _itemsRequested(false)
{
connect(&_points, &QmlObjectListModel::countChanged, this, &RallyPointController::_updateContainsItems);
......@@ -67,7 +69,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
}
_rallyPointManager = _managerVehicle->rallyPointManager();
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_loadComplete);
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &RallyPointController::_managerLoadComplete);
connect(_rallyPointManager, &RallyPointManager::sendComplete, this, &RallyPointController::_managerSendComplete);
connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete);
connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged);
emit rallyPointsSupportedChanged(rallyPointsSupported());
......@@ -123,26 +127,42 @@ void RallyPointController::removeAll(void)
setCurrentRallyPoint(NULL);
}
void RallyPointController::removeAllFromVehicle(void)
{
if (_masterController->offline()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::removeAllFromVehicle called while syncInProgress";
} else {
_rallyPointManager->removeAll();
}
}
void RallyPointController::loadFromVehicle(void)
{
if (!syncInProgress()) {
_rallyPointManager->loadFromVehicle();
if (_masterController->offline()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle called while syncInProgress";
} else {
qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle call while syncInProgress";
_itemsRequested = true;
_rallyPointManager->loadFromVehicle();
}
}
void RallyPointController::sendToVehicle(void)
{
if (!syncInProgress()) {
if (_masterController->offline()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while offline";
} else if (syncInProgress()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while syncInProgress";
} else {
setDirty(false);
QList<QGeoCoordinate> rgPoints;
for (int i=0; i<_points.count(); i++) {
rgPoints.append(qobject_cast<RallyPoint*>(_points[i])->coordinate());
}
_rallyPointManager->sendToVehicle(rgPoints);
} else {
qCWarning(RallyPointControllerLog) << "RallyPointController::loadFromVehicle while syncInProgress";
}
}
......@@ -164,17 +184,36 @@ QString RallyPointController::editorQml(void) const
return _rallyPointManager->editorQml();
}
void RallyPointController::_loadComplete(const QList<QGeoCoordinate> rgPoints)
void RallyPointController::_managerLoadComplete(const QList<QGeoCoordinate> rgPoints)
{
_points.clearAndDeleteContents();
QObjectList pointList;
for (int i=0; i<rgPoints.count(); i++) {
pointList.append(new RallyPoint(rgPoints[i], this));
// Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) {
_points.clearAndDeleteContents();
QObjectList pointList;
for (int i=0; i<rgPoints.count(); i++) {
pointList.append(new RallyPoint(rgPoints[i], this));
}
_points.swapObjectList(pointList);
setDirty(false);
_setFirstPointCurrent();
emit loadComplete();
}
_itemsRequested = false;
}
void RallyPointController::_managerSendComplete(void)
{
// Fly view always reloads after send
if (_editMode) {
showPlanFromManagerVehicle();
}
_points.swapObjectList(pointList);
setDirty(false);
_setFirstPointCurrent();
emit loadComplete();
}
void RallyPointController::_managerRemoveAllComplete(void)
{
// Remove all from vehicle so we always update
showPlanFromManagerVehicle();
}
void RallyPointController::addPoint(QGeoCoordinate point)
......@@ -239,7 +278,27 @@ void RallyPointController::_updateContainsItems(void)
emit containsItemsChanged(containsItems());
}
void RallyPointController::removeAllFromVehicle(void)
bool RallyPointController::showPlanFromManagerVehicle (void)
{
_rallyPointManager->removeAll();
qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle";
if (_masterController->offline()) {
qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline";
return true; // stops further propogation of showPlanFromManagerVehicle due to error
} else {
if (!_managerVehicle->initialPlanRequestComplete()) {
// The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically
qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal";
return true;
} else if (syncInProgress()) {
// If the sync is already in progress, _loadComplete will be called automatically when it is done. So no need to do anything.
qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal";
return true;
} else {
// Fake a _loadComplete with the current items
qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true;
_managerLoadComplete(_rallyPointManager->points());
return false;
}
}
}
......@@ -37,17 +37,18 @@ public:
Q_INVOKABLE void addPoint(QGeoCoordinate point);
Q_INVOKABLE void removePoint(QObject* rallyPoint);
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
void loadFromVehicle (void) final;
void sendToVehicle (void) final;
void removeAll (void) final;
void removeAllFromVehicle (void) final;
bool syncInProgress (void) const final;
bool dirty (void) const final { return _dirty; }
void setDirty (bool dirty) final;
bool containsItems (void) const final;
void managerVehicleChanged (Vehicle* managerVehicle) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
void loadFromVehicle (void) final;
void sendToVehicle (void) final;
void removeAll (void) final;
void removeAllFromVehicle (void) final;
bool syncInProgress (void) const final;
bool dirty (void) const final { return _dirty; }
void setDirty (bool dirty) final;
bool containsItems (void) const final;
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
bool rallyPointsSupported (void) const;
QmlObjectListModel* points (void) { return &_points; }
......@@ -62,7 +63,9 @@ signals:
void loadComplete(void);
private slots:
void _loadComplete(const QList<QGeoCoordinate> rgPoints);
void _managerLoadComplete(const QList<QGeoCoordinate> rgPoints);
void _managerSendComplete(void);
void _managerRemoveAllComplete(void);
void _setFirstPointCurrent(void);
void _updateContainsItems(void);
......@@ -71,6 +74,7 @@ private:
bool _dirty;
QmlObjectListModel _points;
QObject* _currentRallyPoint;
bool _itemsRequested;
static const char* _jsonFileTypeValue;
static const char* _jsonPointsKey;
......
......@@ -41,5 +41,11 @@ void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
// No support in generic vehicle
Q_UNUSED(rgPoints);
emit loadComplete(QList<QGeoCoordinate>());
emit sendComplete();
}
void RallyPointManager::removeAll(void)
{
// No support in generic vehicle
emit removeAllComplete();
}
......@@ -33,12 +33,16 @@ public:
virtual bool inProgress(void) const { return false; }
/// Load the current settings from the vehicle
/// Signals loadComplete when done
virtual void loadFromVehicle(void);
/// Send the current settings to the vehicle
/// Signals sendComplete when done
virtual void sendToVehicle(const QList<QGeoCoordinate>& rgPoints);
virtual void removeAll(void) { };
/// Remove all rally points from the vehicle
/// Signals removeAllCompleted when done
virtual void removeAll(void);
virtual bool rallyPointsSupported(void) const { return false; }
......@@ -58,6 +62,8 @@ signals:
void loadComplete (const QList<QGeoCoordinate> rgPoints);
void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg);
void removeAllComplete (void);
void sendComplete (void);
protected:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
......
......@@ -112,6 +112,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _supportsMissionItemInt(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(NULL)
, _missionManagerInitialRequestSent(false)
, _geoFenceManager(NULL)
......@@ -264,6 +265,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _supportsMissionItemInt(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(NULL)
, _missionManagerInitialRequestSent(false)
, _geoFenceManager(NULL)
......@@ -313,7 +315,7 @@ void Vehicle::_commonInit(void)
_missionManager = new MissionManager(this);
connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);
connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable);
connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
_parameterManager = new ParameterManager(this);
connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
......@@ -321,10 +323,11 @@ void Vehicle::_commonInit(void)
// GeoFenceManager needs to access ParameterManager so make sure to create after
_geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete);
_rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete);
// Offline editing vehicle tracks settings changes for offline editing settings
connect(_settingsManager->appSettings()->offlineEditingFirmwareType(), &Fact::rawValueChanged, this, &Vehicle::_offlineFirmwareTypeSettingChanged);
......@@ -704,7 +707,7 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me
qCDebug(VehicleLog) << "Vehicle supports MISSION_ITEM_INT";
_supportsMissionItemInt = true;
_vehicleCapabilitiesKnown = true;
_startMissionRequest();
_startPlanRequest();
}
}
......@@ -742,7 +745,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) {
// We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request.";
_startMissionRequest();
_startPlanRequest();
}
if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
......@@ -1620,10 +1623,10 @@ void Vehicle::_mapTrajectoryStop()
_mapTrajectoryTimer.stop();
}
void Vehicle::_startMissionRequest(void)
void Vehicle::_startPlanRequest(void)
{
if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
qCDebug(VehicleLog) << "_startMissionRequest";
qCDebug(VehicleLog) << "_startPlanRequest";
_missionManagerInitialRequestSent = true;
if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath();
......@@ -1636,21 +1639,45 @@ void Vehicle::_startMissionRequest(void)
}
}
}
_missionManager->requestMissionItems();
_missionManager->loadFromVehicle();
} else {
if (!_parameterManager->parametersReady()) {
qCDebug(VehicleLog) << "Delaying _startMissionRequest due to parameters not ready";
qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
} else if (!_vehicleCapabilitiesKnown) {
qCDebug(VehicleLog) << "Delaying _startMissionRequest due to vehicle capabilities not know";
qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not know";
}
}
}
void Vehicle::_missionLoadComplete(void)
{
// After the initial mission request completes we ask for the geofence
if (!_geoFenceManagerInitialRequestSent) {
_geoFenceManagerInitialRequestSent = true;
_geoFenceManager->loadFromVehicle();
}
}
void Vehicle::_geoFenceLoadComplete(void)
{
// After geofence request completes we ask for the rally points
if (!_rallyPointManagerInitialRequestSent) {
_rallyPointManagerInitialRequestSent = true;
_rallyPointManager->loadFromVehicle();
}
}
void Vehicle::_rallyPointLoadComplete(void)
{
_initialPlanRequestComplete = true;
}
void Vehicle::_parametersReady(bool parametersReady)
{
if (parametersReady) {
_setupAutoDisarmSignalling();
_startMissionRequest();
_startPlanRequest();
setJoystickEnabled(_joystickEnabled);
}
}
......@@ -2040,7 +2067,7 @@ void Vehicle::_sendMavCommandAgain(void)
if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
// We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
_startMissionRequest();
_startPlanRequest();
}
emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
......@@ -2175,24 +2202,6 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
}
#endif
void Vehicle::_newMissionItemsAvailable(void)
{
// After the initial mission request completes we ask for the geofence
if (!_geoFenceManagerInitialRequestSent) {
_geoFenceManagerInitialRequestSent = true;
_geoFenceManager->loadFromVehicle();
}
}
void Vehicle::_newGeoFenceAvailable(void)
{
// After geofence request completes we ask for the rally points
if (!_rallyPointManagerInitialRequestSent) {
_rallyPointManagerInitialRequestSent = true;
_rallyPointManager->loadFromVehicle();
}
}
QString Vehicle::brandImageIndoor(void) const
{
return _firmwarePlugin->brandImageIndoor(this);
......@@ -2261,7 +2270,7 @@ void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
void Vehicle::triggerCamera(void)
{
sendMavCommand(FactSystem::defaultComponentId,
sendMavCommand(_defaultComponentId,
MAV_CMD_DO_DIGICAM_CONTROL,
true, // show errors
0.0, 0.0, 0.0, 0.0, // param 1-4 unused
......
......@@ -669,6 +669,10 @@ public:
/// @true: When flying a mission the vehicle is always facing towards the next waypoint
bool vehicleYawsToNextWaypointInMission(void) const;
/// The vehicle is responsible for making the initial request for the Plan.
/// @return: true: initial request is complete, false: initial request is still in progress;
bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; }
signals:
void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate);
......@@ -781,8 +785,9 @@ private slots:
void _imageReady (UASInterface* uas);
void _connectionLostTimeout(void);
void _prearmErrorTimeout(void);
void _newMissionItemsAvailable(void);
void _newGeoFenceAvailable(void);
void _missionLoadComplete(void);
void _geoFenceLoadComplete(void);
void _rallyPointLoadComplete(void);
void _sendMavCommandAgain(void);
void _activeJoystickChanged(void);
......@@ -827,7 +832,7 @@ private:
void _sendNextQueuedMavCommand(void);
void _updatePriorityLink(void);
void _commonInit(void);
void _startMissionRequest(void);
void _startPlanRequest(void);
void _setupAutoDisarmSignalling(void);
int _id; ///< Mavlink system id
......@@ -909,6 +914,8 @@ private:
static const int _connectionLostTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of missed heartbeat
QTimer _connectionLostTimer;
bool _initialPlanRequestComplete;
MissionManager* _missionManager;
bool _missionManagerInitialRequestSent;
......
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