Commit 7e320ebd authored by Gus Grubba's avatar Gus Grubba

Merge branch 'master' of https://github.com/mavlink/qgroundcontrol into camControl

parents 26b3b8d0 8a23979e
......@@ -109,6 +109,7 @@
<file alias="compassInstrumentDial.svg">src/FlightMap/Images/compassInstrumentDial.svg</file>
<file alias="crossHair.svg">src/FlightMap/Images/crossHair.svg</file>
<file alias="PiP.svg">src/FlightMap/Images/PiP.svg</file>
<file alias="pipResize.svg">src/FlightMap/Images/pipResize.svg</file>
<file alias="rollDialWhite.svg">src/FlightMap/Images/rollDialWhite.svg</file>
<file alias="rollPointerWhite.svg">src/FlightMap/Images/rollPointerWhite.svg</file>
<file alias="scale.png">src/FlightMap/Images/scale.png</file>
......
......@@ -527,6 +527,7 @@ HEADERS += \
src/MissionManager/SimpleMissionItem.h \
src/MissionManager/Section.h \
src/MissionManager/SpeedSection.h \
src/MissionManager/StructureScanComplexItem.h \
src/MissionManager/SurveyMissionItem.h \
src/MissionManager/VisualMissionItem.h \
src/PositionManager/PositionManager.h \
......@@ -713,6 +714,7 @@ SOURCES += \
src/MissionManager/RallyPointManager.cc \
src/MissionManager/SimpleMissionItem.cc \
src/MissionManager/SpeedSection.cc \
src/MissionManager/StructureScanComplexItem.cc \
src/MissionManager/SurveyMissionItem.cc \
src/MissionManager/VisualMissionItem.cc \
src/PositionManager/PositionManager.cpp \
......
......@@ -115,6 +115,7 @@
<file alias="QGroundControl/Controls/SignalStrength.qml">src/ui/toolbar/SignalStrength.qml</file>
<file alias="QGroundControl/Controls/SimpleItemMapVisual.qml">src/PlanView/SimpleItemMapVisual.qml</file>
<file alias="QGroundControl/Controls/SliderSwitch.qml">src/QmlControls/SliderSwitch.qml</file>
<file alias="QGroundControl/Controls/StructureScanMapVisual.qml">src/PlanView/StructureScanMapVisual.qml</file>
<file alias="QGroundControl/Controls/SubMenuButton.qml">src/QmlControls/SubMenuButton.qml</file>
<file alias="QGroundControl/Controls/SurveyMapVisual.qml">src/PlanView/SurveyMapVisual.qml</file>
<file alias="QGroundControl/Controls/VehicleRotationCal.qml">src/QmlControls/VehicleRotationCal.qml</file>
......@@ -179,7 +180,7 @@
<file alias="SetupParameterEditor.qml">src/VehicleSetup/SetupParameterEditor.qml</file>
<file alias="SetupView.qml">src/VehicleSetup/SetupView.qml</file>
<file alias="SimpleItemEditor.qml">src/PlanView/SimpleItemEditor.qml</file>
<file alias="SurveyItemEditor.qml">src/PlanView/SurveyItemEditor.qml</file>
<file alias="StructureScanEditor.qml">src/PlanView/StructureScanEditor.qml</file>
<file alias="FWLandingPatternEditor.qml">src/PlanView/FWLandingPatternEditor.qml</file>
<file alias="MissionSettingsEditor.qml">src/PlanView/MissionSettingsEditor.qml</file>
<file alias="TcpSettings.qml">src/ui/preferences/TcpSettings.qml</file>
......@@ -206,6 +207,7 @@
<file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
<file alias="StructureScan.SettingsGroup.json">src/MissionManager/StructureScan.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
<file alias="Video.SettingsGroup.json">src/Settings/Video.SettingsGroup.json</file>
<file alias="RallyPoint.FactMetaData.json">src/MissionManager/RallyPoint.FactMetaData.json</file>
......
......@@ -173,7 +173,7 @@ LogDownloadController::_logEntry(UASInterface* uas, uint32_t time_utc, uint32_t
//-- Update this log record
if(num_logs > 0) {
//-- Skip if empty (APM first packet)
if(size) {
if(size || _vehicle->firmwareType() != MAV_AUTOPILOT_ARDUPILOTMEGA) {
id -= _apmOneBased;
if(id < _logEntriesModel.count()) {
QGCLogEntry* entry = _logEntriesModel[id];
......
......@@ -36,7 +36,7 @@ int ULogParser::sizeOfType(QString& typeName)
return 1;
}
qWarning() << "Unkown type in ULog : " << typeName;
qWarning() << "Unknown type in ULog : " << typeName;
return 0;
}
......
......@@ -68,6 +68,7 @@ const char* FactMetaData::_shortDescriptionJsonKey = "shortDescription";
const char* FactMetaData::_longDescriptionJsonKey = "longDescription";
const char* FactMetaData::_unitsJsonKey = "units";
const char* FactMetaData::_defaultValueJsonKey = "defaultValue";
const char* FactMetaData::_mobileDefaultValueJsonKey = "mobileDefaultValue";
const char* FactMetaData::_minJsonKey = "min";
const char* FactMetaData::_maxJsonKey = "max";
const char* FactMetaData::_hasControlJsonKey = "control";
......@@ -971,9 +972,17 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec
if (json.contains(_unitsJsonKey)) {
metaData->setRawUnits(json[_unitsJsonKey].toString());
}
#ifdef __mobile__
if (json.contains(_mobileDefaultValueJsonKey)) {
metaData->setRawDefaultValue(json[_mobileDefaultValueJsonKey].toVariant());
} else if (json.contains(_defaultValueJsonKey)) {
metaData->setRawDefaultValue(json[_defaultValueJsonKey].toVariant());
}
#else
if (json.contains(_defaultValueJsonKey)) {
metaData->setRawDefaultValue(json[_defaultValueJsonKey].toVariant());
}
#endif
if (json.contains(_minJsonKey)) {
QVariant typedValue;
QString errorString;
......
......@@ -260,6 +260,7 @@ private:
static const char* _longDescriptionJsonKey;
static const char* _unitsJsonKey;
static const char* _defaultValueJsonKey;
static const char* _mobileDefaultValueJsonKey;
static const char* _minJsonKey;
static const char* _maxJsonKey;
static const char* _hasControlJsonKey;
......
......@@ -162,12 +162,10 @@ bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities c
uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
if (vehicle->multiRotor()) {
available |= TakeoffVehicleCapability;
} else if (vehicle->fixedWing()) {
// Quad plane supports takeoff
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) &&
vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) {
available |= TakeoffVehicleCapability;
}
} else if (vehicle->fixedWing() || vehicle->vtol()) {
// Due to the way ArduPilot marks a vtol aircraft, we don't know if something is a vtol at initial connection.
// So we always enabled takeoff for fixed wing.
available |= TakeoffVehicleCapability;
}
return (capabilities & available) == capabilities;
......@@ -908,6 +906,11 @@ void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
{
if (!vehicle->multiRotor() && !vehicle->vtol()) {
qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
return false;
}
QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters
......@@ -965,6 +968,8 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
return;
}
} else {
return;
}
}
......
This source diff could not be displayed because it is too large. You can view the blob instead.
......@@ -49,7 +49,7 @@ QGCView {
property bool _isPipVisible: QGroundControl.videoManager.hasVideo ? QGroundControl.loadBoolGlobalSetting(_PIPVisibleKey, true) : false
property real _savedZoomLevel: 0
property real _margins: ScreenTools.defaultFontPixelWidth / 2
property real _pipSize: mainWindow.width * 0.2
property real _pipSize: flightView.width * 0.2
property alias _guidedController: guidedActionsController
property alias _altitudeSlider: altitudeSlider
......@@ -287,6 +287,9 @@ QGCView {
onHideIt: {
setPipVisibility(!state)
}
onNewWidth: {
_pipSize = newWidth
}
}
Row {
......
......@@ -42,8 +42,7 @@ Item {
if(ScreenTools.isMobile) {
return ScreenTools.isTinyScreen ? mainWindow.width * 0.2 : mainWindow.width * 0.15
}
var w = mainWindow.width * 0.15
return Math.min(w, 200)
return ScreenTools.defaultFontPixelWidth * 30
}
function _setInstrumentWidget() {
......
This diff is collapsed.
......@@ -75,6 +75,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
_rgButtonValues[i] = false;
}
_updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
_loadSettings();
connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
......@@ -119,7 +121,7 @@ void Joystick::_setDefaultCalibration(void) {
_saveSettings();
}
void Joystick::_activeVehicleChanged(Vehicle *activeVehicle)
void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle)
{
if(activeVehicle) {
if(activeVehicle->fixedWing()) {
......@@ -137,7 +139,16 @@ void Joystick::_activeVehicleChanged(Vehicle *activeVehicle)
qWarning() << "No valid joystick TXmode settings key for selected vehicle";
return;
}
} else {
_txModeSettingsKey = NULL;
}
}
void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
{
_updateTXModeSettingsKey(activeVehicle);
if(activeVehicle) {
QSettings settings;
settings.beginGroup(_settingsGroup);
int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
......@@ -152,8 +163,10 @@ void Joystick::_loadSettings(void)
settings.beginGroup(_settingsGroup);
if(_txModeSettingsKey)
_transmitterMode = settings.value(_txModeSettingsKey, 2).toInt();
Vehicle* activeVehicle = _multiVehicleManager->activeVehicle();
if(_txModeSettingsKey && activeVehicle)
_transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
settings.beginGroup(_name);
......
......@@ -181,6 +181,7 @@ private:
virtual int _getAxis(int i) = 0;
virtual uint8_t _getHat(int hat,int i) = 0;
void _updateTXModeSettingsKey(Vehicle* activeVehicle);
int _mapFunctionMode(int mode, int function);
void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]);
......
......@@ -65,7 +65,7 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
qCDebug(JoystickLog) << "\t" << name << "axes:" << axisCount << "buttons:" << buttonCount << "hats:" << hatCount << "isGC:" << isGameController;
// Check for joysticks with duplicate names and differentiate the keys when neccessary.
// Check for joysticks with duplicate names and differentiate the keys when necessary.
// This is required when using an Xbox 360 wireless receiver that always identifies as
// 4 individual joysticks, regardless of how many joysticks are actually connected to the
// receiver. Using GUID does not help, all of these devices present the same GUID.
......
......@@ -24,6 +24,7 @@ public:
Q_PROPERTY(double complexDistance READ complexDistance NOTIFY complexDistanceChanged)
/// @return The distance covered the complex mission item in meters.
/// Signals complexDistanceChanged
virtual double complexDistance(void) const = 0;
/// @return Amount of additional time delay in seconds needed to fly the complex item
......@@ -39,6 +40,7 @@ public:
/// Get the point of complex mission item furthest away from a coordinate
/// @param other QGeoCoordinate to which distance is calculated
/// @return the greatest distance from any point of the complex item to some coordinate
/// Signals greatestDistanceToChanged
virtual double greatestDistanceTo(const QGeoCoordinate &other) const = 0;
/// This mission item attribute specifies the type of the complex item.
......@@ -46,6 +48,7 @@ public:
signals:
void complexDistanceChanged (double complexDistance);
void greatestDistanceToChanged (void);
void additionalTimeDelayChanged (double additionalTimeDelay);
};
......
......@@ -18,6 +18,7 @@
#include "SimpleMissionItem.h"
#include "SurveyMissionItem.h"
#include "FixedWingLandingComplexItem.h"
#include "StructureScanComplexItem.h"
#include "JsonHelper.h"
#include "ParameterManager.h"
#include "QGroundControlQmlGlobal.h"
......@@ -60,6 +61,7 @@ MissionController::MissionController(PlanMasterController* masterController, QOb
, _itemsRequested(false)
, _surveyMissionItemName(tr("Survey"))
, _fwLandingMissionItemName(tr("Fixed Wing Landing"))
, _structureScanMissionItemName(tr("Structure Scan"))
, _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings())
, _progressPct(0)
{
......@@ -283,11 +285,11 @@ void MissionController::convertToKMLDocument(QDomDocument& document)
qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_controllerVehicle, item->command());
if (uiInfo && uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) {
coord = QString::number(item->param6()) \
coord = QString::number(item->param6(),'f',7) \
+ "," \
+ QString::number(item->param5()) \
+ QString::number(item->param5(),'f',7) \
+ "," \
+ QString::number(item->param7() + altitude);
+ QString::number(item->param7() + altitude,'f',2);
coords.append(coord);
}
}
......@@ -379,6 +381,8 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
}
} else if (itemName == _fwLandingMissionItemName) {
newItem = new FixedWingLandingComplexItem(_controllerVehicle, _visualItems);
} else if (itemName == _structureScanMissionItemName) {
newItem = new StructureScanComplexItem(_controllerVehicle, _visualItems);
} else {
qWarning() << "Internal error: Unknown complex item:" << itemName;
return sequenceNumber;
......@@ -1475,6 +1479,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
if (complexItem) {
connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(complexItem, &ComplexMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatus);
} else {
qWarning() << "ComplexMissionItem not found";
......@@ -1769,6 +1774,9 @@ QStringList MissionController::complexMissionItemNames(void) const
if (_controllerVehicle->fixedWing()) {
complexItems.append(_fwLandingMissionItemName);
}
if (_controllerVehicle->multiRotor() || _controllerVehicle->vtol()) {
complexItems.append(_structureScanMissionItemName);
}
return complexItems;
}
......
......@@ -231,6 +231,7 @@ private:
MissionFlightStatus_t _missionFlightStatus;
QString _surveyMissionItemName;
QString _fwLandingMissionItemName;
QString _structureScanMissionItemName;
AppSettings* _appSettings;
double _progressPct;
......
......@@ -297,7 +297,7 @@ void MissionItemTest::_testLoadFromJsonV1(void)
QString errorString;
QJsonObject jsonObject = _createV1Json();
// V1 format has param 1-4 in seperate items instead of in params array
// V1 format has param 1-4 in separate items instead of in params array
QStringList removeKeys;
removeKeys << MissionItem::_jsonParam1Key << MissionItem::_jsonParam2Key << MissionItem::_jsonParam3Key << MissionItem::_jsonParam4Key;
......
......@@ -40,8 +40,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
_transactionInProgress = TransactionWrite;
_connectToMavlink();
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;
......
......@@ -38,6 +38,7 @@ PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType)
_ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
connect(_ackTimeoutTimer, &QTimer::timeout, this, &PlanManager::_ackTimeout);
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
}
PlanManager::~PlanManager()
......@@ -62,7 +63,6 @@ void PlanManager::_writeMissionItemsWorker(void)
_transactionInProgress = TransactionWrite;
_retryCount = 0;
emit inProgressChanged(true);
_connectToMavlink();
_writeMissionCount();
}
......@@ -139,7 +139,6 @@ void PlanManager::loadFromVehicle(void)
_retryCount = 0;
_transactionInProgress = TransactionRead;
emit inProgressChanged(true);
_connectToMavlink();
_requestList();
}
......@@ -803,7 +802,6 @@ QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result)
void PlanManager::_finishTransaction(bool success)
{
emit progressPct(1);
_disconnectFromMavlink();
_itemIndicesToRead.clear();
_itemIndicesToWrite.clear();
......@@ -892,7 +890,6 @@ void PlanManager::_removeAllWorker(void)
emit progressPct(0);
_connectToMavlink();
_dedicatedLink = _vehicle->priorityLink();
mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
......@@ -944,16 +941,6 @@ void PlanManager::_clearAndDeleteWriteMissionItems(void)
_writeMissionItems.clear();
}
void PlanManager::_connectToMavlink(void)
{
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
}
void PlanManager::_disconnectFromMavlink(void)
{
disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
}
QString PlanManager::_planTypeString(void)
{
switch (_planType) {
......
......@@ -126,8 +126,6 @@ protected:
void _clearAndDeleteWriteMissionItems(void);
QString _lastMissionReqestString(MAV_MISSION_RESULT result);
void _removeAllWorker(void);
void _connectToMavlink(void);
void _disconnectFromMavlink(void);
QString _planTypeString(void);
protected:
......
......@@ -354,3 +354,13 @@ void QGCMapPolygon::setInteractive(bool interactive)
emit interactiveChanged(interactive);
}
}
QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const
{
if (vertex >= 0 && vertex < _polygonPath.count()) {
return _polygonPath[vertex].value<QGeoCoordinate>();
} else {
qWarning() << "QGCMapPolygon::vertexCoordinate bad vertex requested";
return QGeoCoordinate();
}
}
......@@ -55,6 +55,9 @@ public:
/// Returns the path in a list of QGeoCoordinate's format
QList<QGeoCoordinate> coordinateList(void) const;
/// Returns the QGeoCoordinate for the vertex specified
QGeoCoordinate vertexCoordinate(int vertex) const;
/// Saves the polygon to the json object.
/// @param json Json object to save to
void saveToJson(QJsonObject& json);
......
[
{
"name": "Altitude",
"shortDescription": "Altitude for the bottom layer of the structure scan.",
"type": "double",
"units": "m",
"decimalPlaces": 1,
"defaultValue": 50
},
{
"name": "Layers",
"shortDescription": "Number of scan layers.",
"type": "uint32",
"min": 1,
"defaultValue": 1
},
{
"name": "Layer distance",
"shortDescription": "Distance between each layer.",
"type": "double",
"decimalPlaces": 2,
"min": 0,
"units": "m",
"defaultValue": 25
},
{
"name": "Trigger distance",
"shortDescription": "Distance between each triggering of the camera. 0 specifies not camera trigger.",
"type": "double",
"decimalPlaces": 2,
"min": 0,
"units": "m",
"defaultValue": 25
}
]
This diff is collapsed.
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef StructureScanComplexItem_H
#define StructureScanComplexItem_H
#include "ComplexMissionItem.h"
#include "MissionItem.h"
#include "SettingsFact.h"
#include "QGCLoggingCategory.h"
#include "QGCMapPolygon.h"
Q_DECLARE_LOGGING_CATEGORY(StructureScanComplexItemLog)
class StructureScanComplexItem : public ComplexMissionItem
{
Q_OBJECT
public:
StructureScanComplexItem(Vehicle* vehicle, QObject* parent = NULL);
Q_PROPERTY(Fact* altitude READ altitude CONSTANT)
Q_PROPERTY(Fact* layers READ layers CONSTANT)
Q_PROPERTY(Fact* layerDistance READ layerDistance CONSTANT)
Q_PROPERTY(Fact* cameraTriggerDistance READ cameraTriggerDistance CONSTANT)
Q_PROPERTY(bool altitudeRelative MEMBER _altitudeRelative NOTIFY altitudeRelativeChanged)
Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged)
Q_PROPERTY(double cameraMinTriggerInterval MEMBER _cameraMinTriggerInterval NOTIFY cameraMinTriggerIntervalChanged)
Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT)
Fact* altitude (void) { return &_altitudeFact; }
Fact* layers (void) { return &_layersFact; }
Fact* layerDistance (void) { return &_layerDistanceFact; }
Fact* cameraTriggerDistance (void) { return &_cameraTriggerDistanceFact; }
int cameraShots (void) const;
double timeBetweenShots(void) const;
QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; }
Q_INVOKABLE void rotateEntryPoint(void);
// Overrides from ComplexMissionItem
double complexDistance (void) const final { return _scanDistance; }
int lastSequenceNumber (void) const final;
bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final;
double greatestDistanceTo (const QGeoCoordinate &other) const final;
QString mapVisualQML (void) const final { return QStringLiteral("StructureScanMapVisual.qml"); }
// Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; }
bool isSimpleItem (void) const final { return false; }
bool isStandaloneCoordinate (void) const final { return false; }
bool specifiesCoordinate (void) const final;
bool specifiesAltitudeOnly (void) const final { return false; }
QString commandDescription (void) const final { return tr("Structure Scan"); }
QString commandName (void) const final { return tr("Structure Scan"); }
QString abbreviation (void) const final { return "S"; }
QGeoCoordinate coordinate (void) const final;
QGeoCoordinate exitCoordinate (void) const final;
int sequenceNumber (void) const final { return _sequenceNumber; }
double specifiedFlightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
double specifiedGimbalYaw (void) final { return std::numeric_limits<double>::quiet_NaN(); }
void appendMissionItems (QList<MissionItem*>& items, QObject* missionItemParent) final;
void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final;
void applyNewAltitude (double newAltitude) final;
bool coordinateHasRelativeAltitude (void) const final { return _altitudeRelative; }
bool exitCoordinateHasRelativeAltitude (void) const final { return _altitudeRelative; }
bool exitCoordinateSameAsEntry (void) const final { return true; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { Q_UNUSED(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonArray& missionItems) final;
static const char* jsonComplexItemTypeValue;
signals:
void cameraShotsChanged (int cameraShots);
void timeBetweenShotsChanged (void);
void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval);
void altitudeRelativeChanged (bool altitudeRelative);
private slots:
void _setDirty(void);
void _polygonDirtyChanged(bool dirty);
void _polygonCountChanged(int count);
void _polygonPathChanged(void);
void _clearInternal(void);
void _updateCoordinateAltitudes(void);
private:
void _setExitCoordinate(const QGeoCoordinate& coordinate);
void _setScanDistance(double scanDistance);
void _setCameraShots(int cameraShots);
double _triggerDistance(void) const;
int _sequenceNumber;
bool _dirty;
QGCMapPolygon _mapPolygon;
bool _altitudeRelative;
int _entryVertex; // Polygon vertext which is used as the mission entry point
bool _ignoreRecalc;
double _scanDistance;
int _cameraShots;
double _timeBetweenShots;
double _cameraMinTriggerInterval;
double _cruiseSpeed;
static QMap<QString, FactMetaData*> _metaDataMap;
Fact _altitudeFact;
Fact _layersFact;
Fact _layerDistanceFact;
Fact _cameraTriggerDistanceFact;
static const char* _altitudeFactName;
static const char* _layersFactName;
static const char* _layerDistanceFactName;
static const char* _cameraTriggerDistanceFactName;
static const char* _jsonGridObjectKey;
static const char* _jsonGridAltitudeKey;
static const char* _jsonGridAltitudeRelativeKey;
static const char* _jsonGridAngleKey;
static const char* _jsonGridSpacingKey;
static const char* _jsonGridEntryLocationKey;
static const char* _jsonTurnaroundDistKey;
static const char* _jsonCameraTriggerDistanceKey;
static const char* _jsonCameraTriggerInTurnaroundKey;
static const char* _jsonHoverAndCaptureKey;
static const char* _jsonGroundResolutionKey;
static const char* _jsonFrontalOverlapKey;
static const char* _jsonSideOverlapKey;
static const char* _jsonCameraSensorWidthKey;
static const char* _jsonCameraSensorHeightKey;
static const char* _jsonCameraResolutionWidthKey;
static const char* _jsonCameraResolutionHeightKey;
static const char* _jsonCameraFocalLengthKey;
static const char* _jsonCameraMinTriggerIntervalKey;
static const char* _jsonManualGridKey;
static const char* _jsonCameraObjectKey;
static const char* _jsonCameraNameKey;
static const char* _jsonCameraOrientationLandscapeKey;
static const char* _jsonFixedValueIsAltitudeKey;
static const char* _jsonRefly90DegreesKey;
static const int _hoverAndCaptureDelaySeconds = 1;
};
#endif
......@@ -1322,7 +1322,7 @@ double SurveyMissionItem::timeBetweenShots(void) const
return _cruiseSpeed == 0 ? 0 : _triggerDistance() / _cruiseSpeed;
}
void SurveyMissionItem::setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus)
void SurveyMissionItem::setMissionFlightStatus(MissionController::MissionFlightStatus_t& missionFlightStatus)
{
ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
if (!qFuzzyCompare(_cruiseSpeed, missionFlightStatus.vehicleSpeed)) {
......
......@@ -19,6 +19,7 @@ import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
Rectangle {
id: root
height: ScreenTools.defaultFontPixelHeight * 7
radius: ScreenTools.defaultFontPixelWidth * 0.5
color: qgcPal.window
......@@ -27,8 +28,14 @@ Rectangle {
property var missionItems ///< List of all available mission items
property real maxWidth: parent.width
readonly property real _margins: ScreenTools.defaultFontPixelWidth
onMaxWidthChanged: {
var calcLength = (statusListView.count + 1)*statusListView.contentItem.children[0].width
root.width = root.maxWidth > calcLength ? calcLength : root.maxWidth
}
QGCPalette { id: qgcPal }
QGCLabel {
......@@ -56,6 +63,11 @@ Rectangle {
clip: true
currentIndex: _currentMissionIndex
onCountChanged: {
var calcLength = (statusListView.count + 1)*statusListView.contentItem.children[0].width
root.width = root.maxWidth > calcLength ? calcLength : root.maxWidth
}
delegate: Item {
height: statusListView.height
width: display ? (indicator.width + spacing) : 0
......
......@@ -640,7 +640,7 @@ QGCView {
id: waypointValuesDisplay
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.left: parent.left
anchors.right: rightPanel.left
maxWidth: parent.width - rightPanel.width - x
anchors.bottom: parent.bottom
missionItems: _missionController.visualItems
visible: _editingLayer === _layerMission && !ScreenTools.isShortScreen
......
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Controls.Styles 1.4
import QtQuick.Dialogs 1.2
import QtQuick.Extras 1.4
import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Vehicle 1.0
import QGroundControl.Controls 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Palette 1.0
import QGroundControl.FlightMap 1.0
// Editor for Survery mission items
Rectangle {
id: _root
height: visible ? (editorColumn.height + (_margin * 2)) : 0
width: availableWidth
color: qgcPal.windowShadeDark
radius: _radius
// The following properties must be available up the hierarchy chain
//property real availableWidth ///< Width for control
//property var missionItem ///< Mission Item for editor
property real _margin: ScreenTools.defaultFontPixelWidth / 2
property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle
function polygonCaptureStarted() {
missionItem.clearPolygon()
}
function polygonCaptureFinished(coordinates) {
for (var i=0; i<coordinates.length; i++) {
missionItem.addPolygonCoordinate(coordinates[i])
}
}
function polygonAdjustVertex(vertexIndex, vertexCoordinate) {
missionItem.adjustPolygonCoordinate(vertexIndex, vertexCoordinate)
}
function polygonAdjustStarted() { }
function polygonAdjustFinished() { }
QGCPalette { id: qgcPal; colorGroupEnabled: true }
Column {
id: editorColumn
anchors.margins: _margin
anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("WARNING: WORK IN PROGRESS. USE AT YOUR OWN RISK.")
wrapMode: Text.WordWrap
color: qgcPal.warningText
}
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("WARNING: Photo interval is below minimum interval (%1 secs) supported by camera.").arg(missionItem.cameraMinTriggerInterval.toFixed(1))
wrapMode: Text.WordWrap
color: qgcPal.warningText
visible: missionItem.cameraShots > 0 && missionItem.cameraMinTriggerInterval !== 0 && missionItem.cameraMinTriggerInterval > missionItem.timeBetweenShots
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
columnSpacing: _margin
rowSpacing: _margin
columns: 2
QGCLabel { text: qsTr("Altitude") }
FactTextField {
fact: missionItem.altitude
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layers") }
FactTextField {
fact: missionItem.layers
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layer distance") }
FactTextField {
fact: missionItem.layerDistance
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Trigger Distance") }
FactTextField {
fact: missionItem.cameraTriggerDistance
Layout.fillWidth: true
}
QGCCheckBox {
text: qsTr("Relative altitude")
checked: missionItem.altitudeRelative
Layout.columnSpan: 2
onClicked: missionItem.altitudeRelative = checked
}
}
QGCLabel { text: qsTr("Point camera to structure using:") }
QGCRadioButton { text: qsTr("Vehicle yaw"); enabled: false }
QGCRadioButton { text: qsTr("Gimbal yaw"); checked: true; enabled: false }
QGCButton {
text: qsTr("Rotate entry point")
onClicked: missionItem.rotateEntryPoint()
}
SectionHeader {
id: statsHeader
text: qsTr("Statistics")
}
Grid {
columns: 2
columnSpacing: ScreenTools.defaultFontPixelWidth
visible: statsHeader.checked
QGCLabel { text: qsTr("Photo count") }
QGCLabel { text: missionItem.cameraShots }
QGCLabel { text: qsTr("Photo interval") }
QGCLabel { text: missionItem.timeBetweenShots.toFixed(1) + " " + qsTr("secs") }
}
}
}
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtLocation 5.3
import QtPositioning 5.3
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
import QGroundControl.Controls 1.0
import QGroundControl.FlightMap 1.0
/// Survey Complex Mission Item visuals
Item {
id: _root
property var map ///< Map control to place item in
property var _missionItem: object
property var _mapPolygon: object.mapPolygon
property var _gridComponent
property var _entryCoordinate
property var _exitCoordinate
signal clicked(int sequenceNumber)
function _addVisualElements() {
_gridComponent = gridComponent.createObject(map)
_entryCoordinate = entryPointComponent.createObject(map)
_exitCoordinate = exitPointComponent.createObject(map)
map.addMapItem(_gridComponent)
map.addMapItem(_entryCoordinate)
map.addMapItem(_exitCoordinate)
}
function _destroyVisualElements() {
_gridComponent.destroy()
_entryCoordinate.destroy()
_exitCoordinate.destroy()
}
/// Add an initial 4 sided polygon if there is none
function _addInitialPolygon() {
if (_mapPolygon.count < 3) {
// Initial polygon is inset to take 2/3rds space
var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height)
rect.x += (rect.width * 0.25) / 2
rect.y += (rect.height * 0.25) / 2
rect.width *= 0.75
rect.height *= 0.75
var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */)
var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */)
var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */)
var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
// Initial polygon has max width and height of 3000 meters
var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2
var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2
topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0)
topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0)
bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180)
bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180)
_mapPolygon.appendVertex(topLeftCoord)
_mapPolygon.appendVertex(topRightCoord)
_mapPolygon.appendVertex(bottomRightCoord)
_mapPolygon.appendVertex(bottomLeftCoord)
}
}
Component.onCompleted: {
_addInitialPolygon()
_addVisualElements()
}
Component.onDestruction: {
_destroyVisualElements()
}
QGCMapPolygonVisuals {
id: mapPolygonVisuals
mapControl: map
mapPolygon: _mapPolygon
interactive: _missionItem.isCurrentItem
borderWidth: 1
borderColor: "black"
interiorColor: "green"
interiorOpacity: 0.5
}
// Survey grid lines
Component {
id: gridComponent
MapPolyline {
line.color: "white"
line.width: 2
path: _missionItem.gridPoints
}
}
// Entry point
Component {
id: entryPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.coordinate
visible: _missionItem.exitCoordinate.isValid
sourceItem: MissionItemIndexLabel {
index: _missionItem.sequenceNumber
label: "Entry"
checked: _missionItem.isCurrentItem
onClicked: _root.clicked(_missionItem.sequenceNumber)
}
}
}
// Exit point
Component {
id: exitPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.exitCoordinate
visible: _missionItem.exitCoordinate.isValid
sourceItem: MissionItemIndexLabel {
index: _missionItem.lastSequenceNumber
label: "Exit"
checked: _missionItem.isCurrentItem
onClicked: _root.clicked(_missionItem.sequenceNumber)
}
}
}
}
......@@ -32,7 +32,7 @@ QGCView {
property Fact _editorDialogFact: Fact { }
property int _rowHeight: ScreenTools.defaultFontPixelHeight * 2
property int _rowWidth: 10 // Dynamic adjusted at runtime
property bool _searchFilter: searchText.text != "" ///< true: showing results of search
property bool _searchFilter: searchText.text.trim() != "" ///< true: showing results of search
property var _searchResults ///< List of parameter names from search results
property bool _showRCToParam: !ScreenTools.isMobile && QGroundControl.multiVehicleManager.activeVehicle.px4Firmware
......
......@@ -158,8 +158,9 @@ void ParameterEditorController::setRCToParam(const QString& paramName)
void ParameterEditorController::_updateParameters(void)
{
QObjectList newParameterList;
QStringList searchItems = _searchText.split(' ', QString::SkipEmptyParts);
if (_searchText.isEmpty()) {
if (searchItems.isEmpty()) {
const QMap<int, QMap<QString, QStringList> >& groupMap = _vehicle->parameterManager()->getGroupMap();
foreach (const QString& parameter, groupMap[_currentComponentId][_currentGroup]) {
newParameterList.append(_vehicle->parameterManager()->getParameter(_currentComponentId, parameter));
......@@ -167,9 +168,17 @@ void ParameterEditorController::_updateParameters(void)
} else {
foreach(const QString &parameter, _vehicle->parameterManager()->parameterNames(_vehicle->defaultComponentId())) {
Fact* fact = _vehicle->parameterManager()->getParameter(_vehicle->defaultComponentId(), parameter);
if (fact->name().contains(_searchText, Qt::CaseInsensitive) ||
fact->shortDescription().contains(_searchText, Qt::CaseInsensitive) ||
fact->longDescription().contains(_searchText, Qt::CaseInsensitive)) {
bool matched = true;
// all of the search items must match in order for the parameter to be added to the list
for (const auto& searchItem : searchItems) {
if (!fact->name().contains(searchItem, Qt::CaseInsensitive) &&
!fact->shortDescription().contains(searchItem, Qt::CaseInsensitive) &&
!fact->longDescription().contains(searchItem, Qt::CaseInsensitive)) {
matched = false;
}
}
if (matched) {
newParameterList.append(fact);
}
}
......
......@@ -85,6 +85,11 @@ QGCViewDialog {
}
}
// set focus to the text field when becoming visible (in case of an Enum,
// the valueField is not visible, but it's not an issue because the combo
// box cannot have a focus)
onVisibleChanged: if (visible && !ScreenTools.isMobile) valueField.forceActiveFocus()
QGCFlickable {
anchors.fill: parent
contentHeight: _column.y + _column.height
......
......@@ -22,17 +22,100 @@ Item {
property bool isHidden: false
property bool isDark: false
// As a percentage of the window width
property real maxSize: 0.75
property real minSize: 0.10
signal activated()
signal hideIt(bool state)
signal newWidth(real newWidth)
MouseArea {
id: pipMouseArea
anchors.fill: parent
enabled: !isHidden
hoverEnabled: true
onClicked: {
pip.activated()
}
}
// MouseArea to drag in order to resize the PiP area
MouseArea {
id: pipResize
anchors.top: parent.top
anchors.right: parent.right
height: ScreenTools.minTouchPixels
width: height
property var initialX: 0
property var initialWidth: 0
onClicked: {
// TODO propagate
}
// When we push the mouse button down, we un-anchor the mouse area to prevent a resizing loop
onPressed: {
pipResize.anchors.top = undefined // Top doesn't seem to 'detach'
pipResize.anchors.right = undefined // This one works right, which is what we really need
pipResize.initialX = mouse.x
pipResize.initialWidth = pip.width
}
// When we let go of the mouse button, we re-anchor the mouse area in the correct position
onReleased: {
pipResize.anchors.top = pip.top
pipResize.anchors.right = pip.right
}
// Drag
onPositionChanged: {
if (pipResize.pressed) {
var parentW = pip.parent.width // flightView
var newW = pipResize.initialWidth + mouse.x - pipResize.initialX
if (newW < parentW * maxSize && newW > parentW * minSize) {
newWidth(newW)
}
}
}
}
// Resize icon
Image {
source: "/qmlimages/pipResize.svg"
fillMode: Image.PreserveAspectFit
mipmap: true
anchors.right: parent.right
anchors.top: parent.top
visible: !isHidden && (ScreenTools.isMobile || pipMouseArea.containsMouse)
height: ScreenTools.defaultFontPixelHeight * 2.5
width: ScreenTools.defaultFontPixelHeight * 2.5
sourceSize.height: height
}
// Resize pip window if necessary when main window is resized
property var pipLock: 2
Connections {
target: pip.parent
onWidthChanged: {
// hackity hack...
// don't fire this while app is loading/initializing (it happens twice)
if (pipLock) {
pipLock--
return
}
var parentW = pip.parent.width
if (pip.width > parentW * maxSize) {
newWidth(parentW * maxSize)
} else if (pip.width < parentW * minSize) {
newWidth(parentW * minSize)
}
}
}
//-- PIP Corner Indicator
Image {
id: closePIP
......@@ -41,7 +124,7 @@ Item {
fillMode: Image.PreserveAspectFit
anchors.left: parent.left
anchors.bottom: parent.bottom
visible: !isHidden
visible: !isHidden && (ScreenTools.isMobile || pipMouseArea.containsMouse)
height: ScreenTools.defaultFontPixelHeight * 2.5
width: ScreenTools.defaultFontPixelHeight * 2.5
sourceSize.height: height
......
......@@ -31,7 +31,7 @@ FactPanel {
if (event.key == Qt.Key_Escape) {
reject()
event.accepted = true
} else if (event.key == Qt.Key_Return) {
} else if (event.key == Qt.Key_Return || event.key == Qt.Key_Enter) {
accept()
event.accepted = true
}
......
......@@ -507,6 +507,14 @@ void Vehicle::resetCounters()
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
// if the minimum supported version of MAVLink is already 2.0
// set our max proto version to it.
unsigned mavlinkVersion = _mavlink->getCurrentVersion();
if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) {
_maxProtoVersion = _mavlink->getCurrentVersion();
qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived setting _maxProtoVersion" << _maxProtoVersion;
}
if (message.sysid != _id && message.sysid != 0) {
// We allow RADIO_STATUS messages which come from a link the vehicle is using to pass through and be handled
if (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _containsLink(link))) {
......@@ -516,12 +524,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
if (!_containsLink(link)) {
_addLink(link);
// if the minimum supported version of MAVLink is already 2.0
// set our max proto version to it.
if (_mavlink->getCurrentVersion() >= 200) {
_maxProtoVersion = _mavlink->getCurrentVersion();
}
}
//-- Check link status
......@@ -861,6 +863,7 @@ void Vehicle::_setMaxProtoVersion(unsigned version) {
// Set only once or if we need to reduce the max version
if (_maxProtoVersion == 0 || version < _maxProtoVersion) {
qCDebug(VehicleLog) << "_setMaxProtoVersion before:after" << _maxProtoVersion << version;
_maxProtoVersion = version;
emit requestProtocolVersion(_maxProtoVersion);
......@@ -911,60 +914,6 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
hil.mode);
}
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
bool showError = false;
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
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.";
_setCapabilities(0);
}
if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) {
// The autopilot does not understand the request and consequently is likely handling only
// MAVLink 1
if (_maxProtoVersion == 0) {
_setMaxProtoVersion(100);
}
}
if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
_mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError;
_mavCommandQueue.removeFirst();
}
emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
if (showError) {
QString commandName = _toolbox->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
switch (ack.result) {
case MAV_RESULT_TEMPORARILY_REJECTED:
qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
break;
case MAV_RESULT_DENIED:
qgcApp()->showMessage(tr("%1 command denied").arg(commandName));
break;
case MAV_RESULT_UNSUPPORTED:
qgcApp()->showMessage(tr("%1 command not supported").arg(commandName));
break;
case MAV_RESULT_FAILED:
qgcApp()->showMessage(tr("%1 command failed").arg(commandName));
break;
default:
// Do nothing
break;
}
}
_sendNextQueuedMavCommand();
}
void Vehicle::_handleCommandLong(mavlink_message_t& message)
{
#ifdef __ios__
......@@ -2407,6 +2356,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
qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES. Setting no capabilities. Starting Plan request.";
_setCapabilities(0);
_startPlanRequest();
}
......@@ -2414,8 +2364,12 @@ void Vehicle::_sendMavCommandAgain(void)
if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
// We aren't going to get a response back for the protocol version, so assume v1 is all we can do.
// If the max protocol version is uninitialized, fall back to v1.
qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION. Starting Plan request.";
if (_maxProtoVersion == 0) {
qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set.";
_setMaxProtoVersion(100);
} else {
qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
}
}
......@@ -2487,6 +2441,66 @@ void Vehicle::_sendNextQueuedMavCommand(void)
}
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
bool showError = false;
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
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) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error(%1). Setting no capabilities. Starting Plan request.").arg(ack.result);
_setCapabilities(0);
}
if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) {
// The autopilot does not understand the request and consequently is likely handling only
// MAVLink 1
qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_PROTOCOL_VERSION with error(%1).").arg(ack.result);
if (_maxProtoVersion == 0) {
qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set.";
_setMaxProtoVersion(100);
} else {
qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
}
// FIXME: Is this missing here. I believe it is a bug. Debug to verify. May need to go into Stable.
//_startPlanRequest();
}
if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
_mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError;
_mavCommandQueue.removeFirst();
}
emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
if (showError) {
QString commandName = _toolbox->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
switch (ack.result) {
case MAV_RESULT_TEMPORARILY_REJECTED:
qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
break;
case MAV_RESULT_DENIED:
qgcApp()->showMessage(tr("%1 command denied").arg(commandName));
break;
case MAV_RESULT_UNSUPPORTED:
qgcApp()->showMessage(tr("%1 command not supported").arg(commandName));
break;
case MAV_RESULT_FAILED:
qgcApp()->showMessage(tr("%1 command failed").arg(commandName));
break;
default:
// Do nothing
break;
}
}
_sendNextQueuedMavCommand();
}
void Vehicle::setPrearmError(const QString& prearmError)
{
_prearmError = prearmError;
......
......@@ -382,8 +382,6 @@ void MAVLinkProtocol::_vehicleCountChanged(void)
if (count == 0) {
// Last vehicle is gone, close out logging
_stopLogging();
// Reset protocol version handling
_current_version = 0;
_radio_version_mismatch_count = 0;
}
}
......
#!/bin/sh
astyle \
--style=allman \
--indent=spaces=4 \
--indent-cases \
--indent-preprocessor \
--break-blocks=all \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--align-reference=name \
--suffix=none \
--ignore-exclude-errors-x \
--lineend=linux \
--exclude=EASTL \
$*
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