diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 6cf4e22771ee69ef528d18098505c8cf170e8224..e3c5bdf416c532e22285bb6dc178bff58b738483 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -674,6 +674,7 @@ HEADERS += \
src/SHPFileHelper.h \
src/Terrain/TerrainQuery.h \
src/TerrainTile.h \
+ src/Vehicle/ComponentInformation.h \
src/Vehicle/GPSRTKFactGroup.h \
src/Vehicle/MAVLinkLogManager.h \
src/Vehicle/MultiVehicleManager.h \
@@ -886,6 +887,7 @@ SOURCES += \
src/SHPFileHelper.cc \
src/Terrain/TerrainQuery.cc \
src/TerrainTile.cc\
+ src/Vehicle/ComponentInformation.cc \
src/Vehicle/GPSRTKFactGroup.cc \
src/Vehicle/MAVLinkLogManager.cc \
src/Vehicle/MultiVehicleManager.cc \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 978507ce503d4f7e99051901f2499111854544cb..d5a550e2f0d722c0300b5adc3505ae499017dfa6 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -336,5 +336,6 @@
src/comm/APMArduSubMockLink.params
src/comm/PX4MockLink.params
+ src/comm/MockLink.Version.MetaData.json
diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc
index 9b4575908b8a1005c5498d507444853ee76c4467..4304d4ae802f42ca346bc45bfe1326e80518400f 100644
--- a/src/FactSystem/ParameterManager.cc
+++ b/src/FactSystem/ParameterManager.cc
@@ -112,18 +112,6 @@ ParameterManager::ParameterManager(Vehicle* vehicle)
// Ensure the cache directory exists
QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
-
- if (_vehicle->highLatencyLink()) {
- // High latency links don't load parameters
- _parametersReady = true;
- _missingParameters = true;
- _initialLoadComplete = true;
- _waitingForDefaultComponent = false;
- emit parametersReadyChanged(_parametersReady);
- emit missingParametersChanged(_missingParameters);
- } else if (!_logReplay){
- refreshAllParameters();
- }
}
ParameterManager::~ParameterManager()
@@ -494,8 +482,14 @@ void ParameterManager::_valueUpdated(const QVariant& value)
void ParameterManager::refreshAllParameters(uint8_t componentId)
{
- if (_logReplay) {
- return;
+ if (_vehicle->highLatencyLink() || _logReplay) {
+ // These links don't load params
+ _parametersReady = true;
+ _missingParameters = true;
+ _initialLoadComplete = true;
+ _waitingForDefaultComponent = false;
+ emit parametersReadyChanged(_parametersReady);
+ emit missingParametersChanged(_missingParameters);
}
_dataMutex.lock();
diff --git a/src/Vehicle/ComponentInformation.cc b/src/Vehicle/ComponentInformation.cc
new file mode 100644
index 0000000000000000000000000000000000000000..f1ee2f1a141e13527208db8dba9e85dfdaef06a3
--- /dev/null
+++ b/src/Vehicle/ComponentInformation.cc
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "ComponentInformation.h"
+#include "Vehicle.h"
+
+QGC_LOGGING_CATEGORY(ComponentInformation, "ComponentInformation")
+
+ComponentInformation::ComponentInformation(Vehicle* vehicle, QObject* parent)
+ : QObject (parent)
+ , _vehicle (vehicle)
+{
+
+}
+
+void ComponentInformation::requestVersionMetaData(Vehicle* vehicle)
+{
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_MESSAGE,
+ false, // No error shown if fails
+ MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+ COMP_METADATA_TYPE_VERSION);
+}
+
+bool ComponentInformation::requestParameterMetaData(Vehicle* vehicle)
+{
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_MESSAGE,
+ false, // No error shown if fails
+ MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+ COMP_METADATA_TYPE_PARAMETER);
+ return true;
+}
+
+void ComponentInformation::componentInformationReceived(const mavlink_message_t &message)
+{
+ mavlink_component_information_t componentInformation;
+ mavlink_msg_component_information_decode(&message, &componentInformation);
+ qDebug() << componentInformation.metadata_type << componentInformation.metadata_uri;
+}
+
+bool ComponentInformation::metaDataTypeSupported(COMP_METADATA_TYPE type)
+{
+ return _supportedMetaDataTypes.contains(type);
+}
diff --git a/src/Vehicle/ComponentInformation.h b/src/Vehicle/ComponentInformation.h
new file mode 100644
index 0000000000000000000000000000000000000000..07147840b8993281c770812e11faa95afedc2a02
--- /dev/null
+++ b/src/Vehicle/ComponentInformation.h
@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+
+#include
+
+Q_DECLARE_LOGGING_CATEGORY(ComponentInformationLog)
+
+class Vehicle;
+
+class ComponentInformation : public QObject
+{
+ Q_OBJECT
+
+public:
+ ComponentInformation(Vehicle* vehicle, QObject* parent = nullptr);
+
+ void requestVersionMetaData (Vehicle* vehicle);
+ bool requestParameterMetaData (Vehicle* vehicle);
+ void componentInformationReceived (const mavlink_message_t& message);
+ bool metaDataTypeSupported (COMP_METADATA_TYPE type);
+
+private:
+ Vehicle* _vehicle = nullptr;
+ bool _versionMetaDataAvailable = false;
+ bool _paramMetaDataAvailable = false;
+ QList _supportedMetaDataTypes;
+};
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 7904669cebb4ea9a1ab64aeef89116e74d11015c..9b781de67abcee0a581254fcf8da7d93a8309fc1 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -93,6 +93,18 @@ const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor";
const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus";
const char* Vehicle::_terrainFactGroupName = "terrain";
+const Vehicle::StateFn Vehicle::_rgInitialConnectStates[_cInitialConnectStateEntries] = {
+ Vehicle::_stateRequestCapabilities,
+ Vehicle::_stateRequestProtocolVersion,
+ Vehicle::_stateRequestCompInfoVersion,
+ Vehicle::_stateRequestCompInfoParam,
+ Vehicle::_stateRequestParameters,
+ Vehicle::_stateRequestMission,
+ Vehicle::_stateRequestGeoFence,
+ Vehicle::_stateRequestRallyPoints,
+ Vehicle::_stateSignalInitialConnectComplete
+};
+
// Standard connected vehicle
Vehicle::Vehicle(LinkInterface* link,
int vehicleId,
@@ -153,14 +165,11 @@ Vehicle::Vehicle(LinkInterface* link,
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(nullptr)
- , _missionManagerInitialRequestSent(false)
, _geoFenceManager(nullptr)
- , _geoFenceManagerInitialRequestSent(false)
, _rallyPointManager(nullptr)
- , _rallyPointManagerInitialRequestSent(false)
-#if defined(QGC_AIRMAP_ENABLED)
+ #if defined(QGC_AIRMAP_ENABLED)
, _airspaceVehicleManager(nullptr)
-#endif
+ #endif
, _armed(false)
, _base_mode(0)
, _custom_mode(0)
@@ -190,6 +199,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _orbitActive(false)
, _pidTuningTelemetryMode(false)
, _pidTuningWaitingForRates(false)
+ , _componentInformation(this)
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble)
@@ -267,24 +277,7 @@ Vehicle::Vehicle(LinkInterface* link,
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage);
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived, this, &Vehicle::_handletextMessageReceived);
- if (_highLatencyLink || link->isPX4Flow()) {
- // These links don't request information
- _setMaxProtoVersion(100);
- _setCapabilities(0);
- _initialPlanRequestComplete = true;
- _missionManagerInitialRequestSent = true;
- _geoFenceManagerInitialRequestSent = true;
- _rallyPointManagerInitialRequestSent = true;
- } else {
- sendMavCommand(MAV_COMP_ID_ALL,
- MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
- false, // No error shown if fails
- 1); // Request firmware version
- sendMavCommand(MAV_COMP_ID_ALL,
- MAV_CMD_REQUEST_PROTOCOL_VERSION,
- false, // No error shown if fails
- 1); // Request protocol version
- }
+ _advanceInitialConnectStateMachine();
_firmwarePlugin->initializeVehicle(this);
for(auto& factName: factNames()) {
@@ -360,14 +353,11 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(nullptr)
- , _missionManagerInitialRequestSent(false)
, _geoFenceManager(nullptr)
- , _geoFenceManagerInitialRequestSent(false)
, _rallyPointManager(nullptr)
- , _rallyPointManagerInitialRequestSent(false)
-#if defined(QGC_AIRMAP_ENABLED)
+ #if defined(QGC_AIRMAP_ENABLED)
, _airspaceVehicleManager(nullptr)
-#endif
+ #endif
, _armed(false)
, _base_mode(0)
, _custom_mode(0)
@@ -396,6 +386,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _orbitActive(false)
, _pidTuningTelemetryMode(false)
, _pidTuningWaitingForRates(false)
+ , _componentInformation(this)
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble)
@@ -469,7 +460,7 @@ void Vehicle::_commonInit()
_missionManager = new MissionManager(this);
connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);
- connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
+ connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_firstMissionLoadComplete);
connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints);
connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearCameraTriggerPoints);
connect(_missionManager, &MissionManager::currentIndexChanged, this, &Vehicle::_updateHeadingToNextWP);
@@ -486,11 +477,11 @@ void Vehicle::_commonInit()
// GeoFenceManager needs to access ParameterManager so make sure to create after
_geoFenceManager = new GeoFenceManager(this);
connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
- connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete);
+ connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_firstGeoFenceLoadComplete);
_rallyPointManager = new RallyPointManager(this);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
- connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete);
+ connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_firstRallyPointLoadComplete);
// Flight modes can differ based on advanced mode
connect(_toolbox->corePlugin(), &QGCCorePlugin::showAdvancedUIChanged, this, &Vehicle::flightModesChanged);
@@ -728,31 +719,15 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
return;
}
- if (!_capabilityBitsKnown && message.msgid == MAVLINK_MSG_ID_HEARTBEAT && message.compid == MAV_COMP_ID_AUTOPILOT1) {
- // We want to try to get capabilities as fast as possible so we retry on heartbeats
- bool foundRequest = false;
- for (MavCommandQueueEntry_t& queuedCommand : _mavCommandQueue) {
- if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
- foundRequest = true;
- break;
- }
- }
- if (!foundRequest) {
- _capabilitiesRetryCount++;
- if (_capabilitiesRetryCount == 1) {
- _capabilitiesRetryElapsed.start();
- } else if (_capabilitiesRetryElapsed.elapsed() > 10000){
- qCDebug(VehicleLog) << "Giving up on getting AUTOPILOT_VERSION after 10 seconds";
- qgcApp()->showAppMessage(QStringLiteral("Vehicle failed to send AUTOPILOT_VERSION after waiting/retrying for 10 seconds"));
- _handleUnsupportedRequestAutopilotCapabilities();
- } else {
- // Vehicle never sent us AUTOPILOT_VERSION response. Try again.
- qCDebug(VehicleLog) << QStringLiteral("Sending MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES again due to no response with AUTOPILOT_VERSION - _capabilitiesRetryCount(%1) elapsed(%2)").arg(_capabilitiesRetryCount).arg(_capabilitiesRetryElapsed.elapsed());
- sendMavCommand(MAV_COMP_ID_ALL,
- MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
- true, // Show error on failure
- 1); // Request firmware version
- }
+ if (_waitForMavlinkMessageId != 0) {
+ if (_waitForMavlinkMessageId == message.msgid) {
+ WaitForMavlinkMessageMessageHandler handler = _waitForMavlinkMessageMessageHandler;
+ _waitForMavlinkMessageClear();
+ (*handler)(this, message);
+ } else if (_waitForMavlinkMessageElapsed.elapsed() > _waitForMavlinkMessageTimeoutMsecs) {
+ WaitForMavlinkMessageTimeoutHandler handler = _waitForMavlinkMessageTimeoutHandler;
+ _waitForMavlinkMessageClear();
+ (*handler)(this);
}
}
@@ -802,12 +777,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_COMMAND_LONG:
_handleCommandLong(message);
break;
- case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
- _handleAutopilotVersion(link, message);
- break;
- case MAVLINK_MSG_ID_PROTOCOL_VERSION:
- _handleProtocolVersion(link, message);
- break;
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
break;
@@ -1128,30 +1097,30 @@ void Vehicle::_handleEstimatorStatus(mavlink_message_t& message)
#if 0
typedef enum ESTIMATOR_STATUS_FLAGS
{
- ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */
- ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */
- ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */
- ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */
- ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */
- ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */
- ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */
- ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */
- ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */
- ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */
- ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */
- ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */
+ ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */
+ ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */
+ ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */
+ ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */
+ ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */
+ ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */
+ ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */
+ ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */
+ ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */
+ ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */
+ ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */
+ ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */
typedef struct __mavlink_estimator_status_t {
- uint64_t time_usec; /*< Timestamp (micros since boot or Unix epoch)*/
- float vel_ratio; /*< Velocity innovation test ratio*/
- float pos_horiz_ratio; /*< Horizontal position innovation test ratio*/
- float pos_vert_ratio; /*< Vertical position innovation test ratio*/
- float mag_ratio; /*< Magnetometer innovation test ratio*/
- float hagl_ratio; /*< Height above terrain innovation test ratio*/
- float tas_ratio; /*< True airspeed innovation test ratio*/
- float pos_horiz_accuracy; /*< Horizontal position 1-STD accuracy relative to the EKF local origin (m)*/
- float pos_vert_accuracy; /*< Vertical position 1-STD accuracy relative to the EKF local origin (m)*/
- uint16_t flags; /*< Integer bitmask indicating which EKF outputs are valid. See definition for ESTIMATOR_STATUS_FLAGS.*/
+ uint64_t time_usec; /*< Timestamp (micros since boot or Unix epoch)*/
+ float vel_ratio; /*< Velocity innovation test ratio*/
+ float pos_horiz_ratio; /*< Horizontal position innovation test ratio*/
+ float pos_vert_ratio; /*< Vertical position innovation test ratio*/
+ float mag_ratio; /*< Magnetometer innovation test ratio*/
+ float hagl_ratio; /*< Height above terrain innovation test ratio*/
+ float tas_ratio; /*< True airspeed innovation test ratio*/
+ float pos_horiz_accuracy; /*< Horizontal position 1-STD accuracy relative to the EKF local origin (m)*/
+ float pos_vert_accuracy; /*< Vertical position 1-STD accuracy relative to the EKF local origin (m)*/
+ uint16_t flags; /*< Integer bitmask indicating which EKF outputs are valid. See definition for ESTIMATOR_STATUS_FLAGS.*/
} mavlink_estimator_status_t;
};
#endif
@@ -1453,72 +1422,6 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits)
_setMaxProtoVersionFromBothSources();
}
-void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
-{
- Q_UNUSED(link);
-
- mavlink_autopilot_version_t autopilotVersion;
- mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
-
- _uid = (quint64)autopilotVersion.uid;
- emit vehicleUIDChanged();
-
- if (autopilotVersion.flight_sw_version != 0) {
- int majorVersion, minorVersion, patchVersion;
- FIRMWARE_VERSION_TYPE versionType;
-
- majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF;
- minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF;
- patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF;
- versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF);
- setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
- }
-
- if (px4Firmware()) {
- // Lower 3 bytes is custom version
- int majorVersion, minorVersion, patchVersion;
- majorVersion = autopilotVersion.flight_custom_version[2];
- minorVersion = autopilotVersion.flight_custom_version[1];
- patchVersion = autopilotVersion.flight_custom_version[0];
- setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion);
-
- // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order
- _gitHash = "";
- for (int i = 7; i >= 0; i--) {
- _gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0')));
- }
- } else {
- // APM Firmware stores the first 8 characters of the git hash as an ASCII character string
- char nullStr[9];
- strncpy(nullStr, (char*)autopilotVersion.flight_custom_version, 8);
- nullStr[8] = 0;
- _gitHash = nullStr;
- }
- if (_toolbox->corePlugin()->options()->checkFirmwareVersion() && !_checkLatestStableFWDone) {
- _checkLatestStableFWDone = true;
- _firmwarePlugin->checkIfIsLatestStable(this);
- }
- emit gitHashChanged(_gitHash);
-
- _setCapabilities(autopilotVersion.capabilities);
- _startPlanRequest();
-}
-
-void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& message)
-{
- Q_UNUSED(link);
-
- mavlink_protocol_version_t protoVersion;
- mavlink_msg_protocol_version_decode(&message, &protoVersion);
-
- qCDebug(VehicleLog) << "_handleProtocolVersion" << protoVersion.max_version;
-
- _mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version;
- _mavlinkProtocolRequestComplete = true;
- _setMaxProtoVersionFromBothSources();
- _startPlanRequest();
-}
-
void Vehicle::_setMaxProtoVersion(unsigned version) {
// Set only once or if we need to reduce the max version
@@ -1547,7 +1450,7 @@ QString Vehicle::vehicleUIDStr()
QString uid;
uint8_t* pUid = (uint8_t*)(void*)&_uid;
uid.asprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
- pUid[0] & 0xff,
+ pUid[0] & 0xff,
pUid[1] & 0xff,
pUid[2] & 0xff,
pUid[3] & 0xff,
@@ -2034,11 +1937,11 @@ void Vehicle::_handleRCChannelsRaw(mavlink_message_t& message)
// Pop warnings ignoring for mavlink headers for both GCC/Clang and MSVC
#ifdef __GNUC__
- #if defined(__clang__)
- #pragma clang diagnostic pop
- #else
- #pragma GCC diagnostic pop
- #endif
+#if defined(__clang__)
+#pragma clang diagnostic pop
+#else
+#pragma GCC diagnostic pop
+#endif
#else
#pragma warning(pop, 0)
#endif
@@ -2667,81 +2570,24 @@ void Vehicle::_updateFlightTime()
_flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0);
}
-void Vehicle::_startPlanRequest()
-{
- if (_missionManagerInitialRequestSent) {
- // We have already started (or possibly completed) the sequence of requesting the plan for the first time
- return;
- }
-
- // We don't start the Plan request until the following things are satisfied:
- // - Parameter download is complete
- // - We know the vehicle capabilities
- // - We know the max mavlink protocol version
- if (_parameterManager->parametersReady() && _capabilityBitsKnown && _mavlinkProtocolRequestComplete) {
- qCDebug(VehicleLog) << "_startPlanRequest";
- _missionManagerInitialRequestSent = true;
- if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
- QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath();
- if (!missionAutoLoadDirPath.isEmpty()) {
- QDir missionAutoLoadDir(missionAutoLoadDirPath);
- QString autoloadFilename = missionAutoLoadDir.absoluteFilePath(tr("AutoLoad%1.%2").arg(_id).arg(AppSettings::planFileExtension));
- if (QFile(autoloadFilename).exists()) {
- _initialPlanRequestComplete = true; // We aren't going to load from the vehicle, so we are done
- PlanMasterController::sendPlanToVehicle(this, autoloadFilename);
- return;
- }
- }
- }
- _missionManager->loadFromVehicle();
- } else {
- if (!_parameterManager->parametersReady()) {
- qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
- } else if (!_capabilityBitsKnown) {
- qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
- } else if (!_mavlinkProtocolRequestComplete) {
- qCDebug(VehicleLog) << "Delaying _startPlanRequest due to mavlink protocol request not complete";
- }
- }
-}
-
-void Vehicle::_missionLoadComplete()
+void Vehicle::_firstMissionLoadComplete()
{
- // After the initial mission request completes we ask for the geofence
- if (!_geoFenceManagerInitialRequestSent) {
- _geoFenceManagerInitialRequestSent = true;
- if (_geoFenceManager->supported()) {
- qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
- _geoFenceManager->loadFromVehicle();
- } else {
- qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
- _geoFenceLoadComplete();
- }
- }
+ disconnect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_firstMissionLoadComplete);
+ _advanceInitialConnectStateMachine();
}
-void Vehicle::_geoFenceLoadComplete()
+void Vehicle::_firstGeoFenceLoadComplete()
{
- // After geofence request completes we ask for the rally points
- if (!_rallyPointManagerInitialRequestSent) {
- _rallyPointManagerInitialRequestSent = true;
- if (_rallyPointManager->supported()) {
- qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
- _rallyPointManager->loadFromVehicle();
- } else {
- qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
- _rallyPointLoadComplete();
- }
- }
+ disconnect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_firstGeoFenceLoadComplete);
+ _advanceInitialConnectStateMachine();
}
-void Vehicle::_rallyPointLoadComplete()
+void Vehicle::_firstRallyPointLoadComplete()
{
- qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
- if (!_initialPlanRequestComplete) {
- _initialPlanRequestComplete = true;
- emit initialPlanRequestCompleteChanged(true);
- }
+ disconnect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_firstRallyPointLoadComplete);
+ _initialPlanRequestComplete = true;
+ emit initialPlanRequestCompleteChanged(true);
+ _advanceInitialConnectStateMachine();
}
void Vehicle::_parametersReady(bool parametersReady)
@@ -2753,7 +2599,7 @@ void Vehicle::_parametersReady(bool parametersReady)
_sendQGCTimeToVehicle();
if (parametersReady) {
_setupAutoDisarmSignalling();
- _startPlanRequest();
+ _advanceInitialConnectStateMachine();
}
}
@@ -2832,11 +2678,11 @@ void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw,
// The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled
if (!_joystickEnabled) {
sendJoystickDataThreadSafe(
- static_cast(roll),
- static_cast(pitch),
- static_cast(yaw),
- static_cast(thrust),
- 0);
+ static_cast(roll),
+ static_cast(pitch),
+ static_cast(yaw),
+ static_cast(thrust),
+ 0);
}
}
@@ -2869,12 +2715,6 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID
if (_priorityLink->highLatency()) {
_setMaxProtoVersion(100);
- } else {
- // Re-negotiate protocol version for the link
- sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet.
- MAV_CMD_REQUEST_PROTOCOL_VERSION,
- false, // No error shown if fails
- 1); // Request protocol version
}
} else if (!active && !_connectionLost) {
_updatePriorityLink(false /* updateActive */, false /* sendCommand */);
@@ -3164,27 +3004,27 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius,
}
if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
sendMavCommandInt(
- defaultComponentId(),
- MAV_CMD_DO_ORBIT,
- MAV_FRAME_GLOBAL,
- true, // show error if fails
- static_cast(radius),
- static_cast(qQNaN()), // Use default velocity
- 0, // Vehicle points to center
- static_cast(qQNaN()), // reserved
- centerCoord.latitude(), centerCoord.longitude(), static_cast(amslAltitude));
+ defaultComponentId(),
+ MAV_CMD_DO_ORBIT,
+ MAV_FRAME_GLOBAL,
+ true, // show error if fails
+ static_cast(radius),
+ static_cast(qQNaN()), // Use default velocity
+ 0, // Vehicle points to center
+ static_cast(qQNaN()), // reserved
+ centerCoord.latitude(), centerCoord.longitude(), static_cast(amslAltitude));
} else {
sendMavCommand(
- defaultComponentId(),
- MAV_CMD_DO_ORBIT,
- true, // show error if fails
- static_cast(radius),
- static_cast(qQNaN()), // Use default velocity
- 0, // Vehicle points to center
- static_cast(qQNaN()), // reserved
- static_cast(centerCoord.latitude()),
- static_cast(centerCoord.longitude()),
- static_cast(amslAltitude));
+ defaultComponentId(),
+ MAV_CMD_DO_ORBIT,
+ true, // show error if fails
+ static_cast(radius),
+ static_cast(qQNaN()), // Use default velocity
+ 0, // Vehicle points to center
+ static_cast(qQNaN()), // reserved
+ static_cast(centerCoord.latitude()),
+ static_cast(centerCoord.longitude()),
+ static_cast(amslAltitude));
}
}
@@ -3196,29 +3036,29 @@ void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord)
}
if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
sendMavCommandInt(
- defaultComponentId(),
- MAV_CMD_DO_SET_ROI_LOCATION,
- MAV_FRAME_GLOBAL,
- true, // show error if fails
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- centerCoord.latitude(),
- centerCoord.longitude(),
- static_cast(centerCoord.altitude()));
+ defaultComponentId(),
+ MAV_CMD_DO_SET_ROI_LOCATION,
+ MAV_FRAME_GLOBAL,
+ true, // show error if fails
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ centerCoord.latitude(),
+ centerCoord.longitude(),
+ static_cast(centerCoord.altitude()));
} else {
sendMavCommand(
- defaultComponentId(),
- MAV_CMD_DO_SET_ROI_LOCATION,
- true, // show error if fails
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(centerCoord.latitude()),
- static_cast(centerCoord.longitude()),
- static_cast(centerCoord.altitude()));
+ defaultComponentId(),
+ MAV_CMD_DO_SET_ROI_LOCATION,
+ true, // show error if fails
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(centerCoord.latitude()),
+ static_cast(centerCoord.longitude()),
+ static_cast(centerCoord.altitude()));
}
}
@@ -3230,29 +3070,29 @@ void Vehicle::stopGuidedModeROI()
}
if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
sendMavCommandInt(
- defaultComponentId(),
- MAV_CMD_DO_SET_ROI_NONE,
- MAV_FRAME_GLOBAL,
- true, // show error if fails
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN())); // Empty
+ defaultComponentId(),
+ MAV_CMD_DO_SET_ROI_NONE,
+ MAV_FRAME_GLOBAL,
+ true, // show error if fails
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN())); // Empty
} else {
sendMavCommand(
- defaultComponentId(),
- MAV_CMD_DO_SET_ROI_NONE,
- true, // show error if fails
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN()), // Empty
- static_cast(qQNaN())); // Empty
+ defaultComponentId(),
+ MAV_CMD_DO_SET_ROI_NONE,
+ true, // show error if fails
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN()), // Empty
+ static_cast(qQNaN())); // Empty
}
}
@@ -3268,10 +3108,10 @@ void Vehicle::pauseVehicle()
void Vehicle::abortLanding(double climbOutAltitude)
{
sendMavCommand(
- defaultComponentId(),
- MAV_CMD_DO_GO_AROUND,
- true, // show error if fails
- static_cast(climbOutAltitude));
+ defaultComponentId(),
+ MAV_CMD_DO_GO_AROUND,
+ true, // show error if fails
+ static_cast(climbOutAltitude));
}
bool Vehicle::guidedMode() const
@@ -3287,11 +3127,11 @@ void Vehicle::setGuidedMode(bool guidedMode)
void Vehicle::emergencyStop()
{
sendMavCommand(
- _defaultComponentId,
- MAV_CMD_COMPONENT_ARM_DISARM,
- true, // show error if fails
- 0.0f,
- 21196.0f); // Magic number for emergency stop
+ _defaultComponentId,
+ MAV_CMD_COMPONENT_ARM_DISARM,
+ true, // show error if fails
+ 0.0f,
+ 21196.0f); // Magic number for emergency stop
}
void Vehicle::setCurrentMissionSequence(int seq)
@@ -3301,13 +3141,13 @@ void Vehicle::setCurrentMissionSequence(int seq)
}
mavlink_message_t msg;
mavlink_msg_mission_set_current_pack_chan(
- static_cast(_mavlink->getSystemId()),
- static_cast(_mavlink->getComponentId()),
- priorityLink()->mavlinkChannel(),
- &msg,
- static_cast(id()),
- _compID,
- static_cast(seq));
+ static_cast(_mavlink->getSystemId()),
+ static_cast(_mavlink->getComponentId()),
+ priorityLink()->mavlinkChannel(),
+ &msg,
+ static_cast(id()),
+ _compID,
+ static_cast(seq));
sendMessageOnLinkThreadSafe(priorityLink(), msg);
}
@@ -3319,6 +3159,32 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo
entry.component = component;
entry.command = command;
entry.showError = showError;
+ entry.resultHandler = nullptr;
+ entry.rgParam[0] = static_cast(param1);
+ entry.rgParam[1] = static_cast(param2);
+ entry.rgParam[2] = static_cast(param3);
+ entry.rgParam[3] = static_cast(param4);
+ entry.rgParam[4] = static_cast(param5);
+ entry.rgParam[5] = static_cast(param6);
+ entry.rgParam[6] = static_cast(param7);
+
+ _mavCommandQueue.append(entry);
+
+ if (_mavCommandQueue.count() == 1) {
+ _mavCommandRetryCount = 0;
+ _sendMavCommandAgain();
+ }
+}
+
+void Vehicle::sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler resultHandler, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+{
+ MavCommandQueueEntry_t entry;
+
+ entry.commandInt = false;
+ entry.component = component;
+ entry.command = command;
+ entry.showError = false;
+ entry.resultHandler = resultHandler;
entry.rgParam[0] = static_cast(param1);
entry.rgParam[1] = static_cast(param2);
entry.rgParam[2] = static_cast(param3);
@@ -3344,6 +3210,7 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame,
entry.command = command;
entry.frame = frame;
entry.showError = showError;
+ entry.resultHandler = nullptr;
entry.rgParam[0] = static_cast(param1);
entry.rgParam[1] = static_cast(param2);
entry.rgParam[2] = static_cast(param3);
@@ -3371,17 +3238,10 @@ void Vehicle::_sendMavCommandAgain()
MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
- if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
- qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES.";
- _handleUnsupportedRequestAutopilotCapabilities();
- }
-
- if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
- qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION.";
- _handleUnsupportedRequestProtocolVersion();
- }
-
emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+ if (queuedCommand.resultHandler) {
+ (*queuedCommand.resultHandler)(this, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
+ }
if (queuedCommand.showError) {
qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command)));
}
@@ -3457,68 +3317,13 @@ void Vehicle::_sendNextQueuedMavCommand()
}
}
-void Vehicle::_handleUnsupportedRequestProtocolVersion()
-{
- // We end up here if either the vehicle does not support the MAV_CMD_REQUEST_PROTOCOL_VERSION command or if
- // we never received an Ack back for the command.
-
- // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
- _mavlinkProtocolRequestComplete = true;
- _setMaxProtoVersionFromBothSources();
-
- // Determining protocol version is one of the triggers to possibly start downloading the plan
- _startPlanRequest();
-}
-
-void Vehicle::_protocolVersionTimeOut()
-{
- // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC.
- // This means although the vehicle may support mavlink 2, the pipe does not.
- qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message.");
- _mavlinkProtocolRequestMaxProtoVersion = 100;
- _mavlinkProtocolRequestComplete = true;
- _setMaxProtoVersionFromBothSources();
- _startPlanRequest();
-}
-
-void Vehicle::_handleUnsupportedRequestAutopilotCapabilities()
-{
- // We end up here if either the vehicle does not support the MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES command or if
- // we never received an Ack back for the command.
-
- _setCapabilities(0);
-
- // Determining vehicle capabilities is one of the triggers to possibly start downloading the plan
- _startPlanRequest();
-}
-
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
- bool showError = false;
-
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
qCDebug(VehicleLog) << QStringLiteral("_handleCommandAck command(%1) result(%2)").arg(ack.command).arg(ack.result);
- if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) {
- qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error(%1). Setting no capabilities.").arg(ack.result);
- _handleUnsupportedRequestAutopilotCapabilities();
- }
-
- if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
- if (ack.result == MAV_RESULT_ACCEPTED) {
- // The vehicle should be sending a PROTOCOL_VERSION message in a mavlink 2 packet. This may or may not make it through the pipe.
- // So we wait for it to come and timeout if it doesn't.
- if (!_mavlinkProtocolRequestComplete) {
- QTimer::singleShot(1000, this, &Vehicle::_protocolVersionTimeOut);
- }
- } else {
- qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_PROTOCOL_VERSION with error(%1).").arg(ack.result);
- _handleUnsupportedRequestProtocolVersion();
- }
- }
-
if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) {
if (ack.result == MAV_RESULT_ACCEPTED) {
_isROIEnabled = true;
@@ -3539,14 +3344,20 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
}
#endif
+ bool showError = false;
+ MavCmdResultHandler resultHandler = nullptr;
if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
_mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError;
+ resultHandler = _mavCommandQueue[0].resultHandler;
_mavCommandQueue.removeFirst();
_sendNextQueuedMavCommand();
}
emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
+ if (resultHandler) {
+ (*resultHandler)(this, static_cast(ack.result), false /* noResponsefromVehicle */);
+ }
if (showError) {
QString commandName = _toolbox->missionCommandTree()->friendlyName(static_cast(ack.command));
@@ -4233,7 +4044,7 @@ void Vehicle::_writeCsvLine()
{
// Only save the logs after the the vehicle gets armed, unless "Save logs even if vehicle was not armed" is checked
if(!_csvLogFile.isOpen() &&
- (_armed || _toolbox->settingsManager()->appSettings()->telemetrySaveNotArmed()->rawValue().toBool())){
+ (_armed || _toolbox->settingsManager()->appSettings()->telemetrySaveNotArmed()->rawValue().toBool())){
_initializeCsv();
}
@@ -4276,16 +4087,16 @@ void Vehicle::gimbalControlValue(double pitch, double yaw)
{
//qDebug() << "Gimbal:" << pitch << yaw;
sendMavCommand(
- _defaultComponentId,
- MAV_CMD_DO_MOUNT_CONTROL,
- false, // show errors
- static_cast(pitch), // Pitch 0 - 90
- 0, // Roll (not used)
- static_cast(yaw), // Yaw -180 - 180
- 0, // Altitude (not used)
- 0, // Latitude (not used)
- 0, // Longitude (not used)
- MAV_MOUNT_MODE_MAVLINK_TARGETING); // MAVLink Roll,Pitch,Yaw
+ _defaultComponentId,
+ MAV_CMD_DO_MOUNT_CONTROL,
+ false, // show errors
+ static_cast(pitch), // Pitch 0 - 90
+ 0, // Roll (not used)
+ static_cast(yaw), // Yaw -180 - 180
+ 0, // Altitude (not used)
+ 0, // Latitude (not used)
+ 0, // Longitude (not used)
+ MAV_MOUNT_MODE_MAVLINK_TARGETING); // MAVLink Roll,Pitch,Yaw
}
void Vehicle::gimbalPitchStep(int direction)
@@ -4337,9 +4148,9 @@ void Vehicle::_handleGimbalOrientation(const mavlink_message_t& message)
void Vehicle::_handleObstacleDistance(const mavlink_message_t& message)
{
- mavlink_obstacle_distance_t o;
- mavlink_msg_obstacle_distance_decode(&message, &o);
- _objectAvoidance->update(&o);
+ mavlink_obstacle_distance_t o;
+ mavlink_msg_obstacle_distance_decode(&message, &o);
+ _objectAvoidance->update(&o);
}
void Vehicle::updateFlightDistance(double distance)
@@ -4411,19 +4222,341 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo
float newThrustCommand = thrust * axesScaling;
mavlink_msg_manual_control_pack_chan(
- static_cast(_mavlink->getSystemId()),
- static_cast(_mavlink->getComponentId()),
- priorityLink()->mavlinkChannel(),
- &message,
- static_cast(_id),
- static_cast(newPitchCommand),
- static_cast(newRollCommand),
- static_cast(newThrustCommand),
- static_cast(newYawCommand),
- buttons);
+ static_cast(_mavlink->getSystemId()),
+ static_cast(_mavlink->getComponentId()),
+ priorityLink()->mavlinkChannel(),
+ &message,
+ static_cast(_id),
+ static_cast(newPitchCommand),
+ static_cast(newRollCommand),
+ static_cast(newThrustCommand),
+ static_cast(newYawCommand),
+ buttons);
sendMessageOnLinkThreadSafe(priorityLink(), message);
}
+void Vehicle::_advanceInitialConnectStateMachine(void)
+{
+ _currentInitialConnectState++;
+ if (_currentInitialConnectState < _cInitialConnectStateEntries) {
+ (*_rgInitialConnectStates[_currentInitialConnectState])(this);
+ }
+}
+
+void Vehicle::_advanceInitialConnectStateMachine(StateFn stateFn)
+{
+ for (int i=0; i<_cInitialConnectStateEntries; i++) {
+ if (_rgInitialConnectStates[i] == stateFn) {
+ _currentInitialConnectState = i;
+ (*_rgInitialConnectStates[_currentInitialConnectState])(this);
+ break;
+ }
+ }
+}
+
+void Vehicle::_waitForMavlinkMessage(int messageId, int timeoutMsecs, WaitForMavlinkMessageMessageHandler messageHandler, WaitForMavlinkMessageTimeoutHandler timeoutHandler)
+{
+ _waitForMavlinkMessageId = messageId;
+ _waitForMavlinkMessageTimeoutMsecs = timeoutMsecs;
+ _waitForMavlinkMessageTimeoutHandler = timeoutHandler;
+ _waitForMavlinkMessageMessageHandler = messageHandler;
+ _waitForMavlinkMessageElapsed.restart();
+}
+
+void Vehicle::_waitForMavlinkMessageClear(void)
+{
+ _waitForMavlinkMessageId = 0;
+ _waitForMavlinkMessageTimeoutMsecs = 0;
+ _waitForMavlinkMessageTimeoutHandler = nullptr;
+ _waitForMavlinkMessageMessageHandler = nullptr;
+}
+
+void Vehicle::_stateRequestCapabilities(Vehicle* vehicle)
+{
+ LinkInterface* link = vehicle->priorityLink();
+ if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+ qCDebug(VehicleLog) << "Skipping capability request due to link type";
+ vehicle->_advanceInitialConnectStateMachine();
+ } else {
+ qCDebug(VehicleLog) << "Requesting capabilities";
+ vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_AUTOPILOT_VERSION, 1000, _waitForAutopilotVersionMessageHandler, _waitForAutopilotVersionTimeoutHandler);
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+ _capabilitiesCmdResultHandler,
+ 1); // Request firmware version
+ }
+}
+
+void Vehicle::_capabilitiesCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
+{
+ if (result != MAV_RESULT_ACCEPTED) {
+ if (noResponsefromVehicle) {
+ qCDebug(VehicleLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle";
+ } else {
+ qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result);
+ }
+ qCDebug(VehicleLog) << "Setting no capabilities";
+ vehicle->_setCapabilities(0);
+ vehicle->_waitForMavlinkMessageClear();
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestProtocolVersion);
+ }
+}
+
+void Vehicle::_waitForAutopilotVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
+{
+ qCDebug(VehicleLog) << "AUTOPILOT_VERSION received";
+
+ mavlink_autopilot_version_t autopilotVersion;
+ mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);
+
+ vehicle->_uid = (quint64)autopilotVersion.uid;
+ emit vehicle->vehicleUIDChanged();
+
+ if (autopilotVersion.flight_sw_version != 0) {
+ int majorVersion, minorVersion, patchVersion;
+ FIRMWARE_VERSION_TYPE versionType;
+
+ majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF;
+ minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF;
+ patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF;
+ versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF);
+ vehicle->setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
+ }
+
+ if (vehicle->px4Firmware()) {
+ // Lower 3 bytes is custom version
+ int majorVersion, minorVersion, patchVersion;
+ majorVersion = autopilotVersion.flight_custom_version[2];
+ minorVersion = autopilotVersion.flight_custom_version[1];
+ patchVersion = autopilotVersion.flight_custom_version[0];
+ vehicle->setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion);
+
+ // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order
+ vehicle->_gitHash = "";
+ for (int i = 7; i >= 0; i--) {
+ vehicle->_gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0')));
+ }
+ } else {
+ // APM Firmware stores the first 8 characters of the git hash as an ASCII character string
+ char nullStr[9];
+ strncpy(nullStr, (char*)autopilotVersion.flight_custom_version, 8);
+ nullStr[8] = 0;
+ vehicle->_gitHash = nullStr;
+ }
+ if (vehicle->_toolbox->corePlugin()->options()->checkFirmwareVersion() && !vehicle->_checkLatestStableFWDone) {
+ vehicle->_checkLatestStableFWDone = true;
+ vehicle->_firmwarePlugin->checkIfIsLatestStable(vehicle);
+ }
+ emit vehicle->gitHashChanged(vehicle->_gitHash);
+
+ vehicle->_setCapabilities(autopilotVersion.capabilities);
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+void Vehicle::_waitForAutopilotVersionTimeoutHandler(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "AUTOPILOT_VERSION timeout";
+ qCDebug(VehicleLog) << "Setting no capabilities";
+ vehicle->_setCapabilities(0);
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+void Vehicle::_stateRequestProtocolVersion(Vehicle* vehicle)
+{
+ LinkInterface* link = vehicle->priorityLink();
+ if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+ qCDebug(VehicleLog) << "Skipping protocol version request due to link type";
+ vehicle->_advanceInitialConnectStateMachine();
+ } else {
+ qCDebug(VehicleLog) << "Requesting protocol version";
+ vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_PROTOCOL_VERSION, 1000, _waitForProtocolVersionMessageHandler, _waitForProtocolVersionTimeoutHandler);
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_PROTOCOL_VERSION,
+ _protocolVersionCmdResultHandler,
+ 1); // Request protocol version
+ }
+}
+
+void Vehicle::_protocolVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
+{
+ if (result != MAV_RESULT_ACCEPTED) {
+ if (noResponsefromVehicle) {
+ qCDebug(VehicleLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle";
+ } else {
+ qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result);
+ }
+
+ // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown
+ vehicle->_mavlinkProtocolRequestComplete = true;
+ vehicle->_setMaxProtoVersionFromBothSources();
+ vehicle->_waitForMavlinkMessageClear();
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestCompInfoVersion);
+ }
+}
+
+void Vehicle::_waitForProtocolVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
+{
+ mavlink_protocol_version_t protoVersion;
+ mavlink_msg_protocol_version_decode(&message, &protoVersion);
+
+ qCDebug(VehicleLog) << "PROTOCOL_VERSION received mav_version:" << protoVersion.max_version;
+ vehicle->_mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version;
+ vehicle->_mavlinkProtocolRequestComplete = true;
+ vehicle->_setMaxProtoVersionFromBothSources();
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+
+void Vehicle::_waitForProtocolVersionTimeoutHandler(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "PROTOCOL_VERSION timeout";
+ // The PROTOCOL_VERSION message didn't make it through the pipe from Vehicle->QGC.
+ // This means although the vehicle may support mavlink 2, the pipe does not.
+ qCDebug(VehicleLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message.");
+ vehicle->_mavlinkProtocolRequestMaxProtoVersion = 100;
+ vehicle->_mavlinkProtocolRequestComplete = true;
+ vehicle->_setMaxProtoVersionFromBothSources();
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+
+void Vehicle::_stateRequestCompInfoVersion(Vehicle* vehicle)
+{
+ LinkInterface* link = vehicle->priorityLink();
+ if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+ qCDebug(VehicleLog) << "Skipping version component information request due to link type";
+ vehicle->_advanceInitialConnectStateMachine();
+ } else {
+ qCDebug(VehicleLog) << "Requesting version component information";
+ vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_COMPONENT_INFORMATION, 1000, _waitForCompInfoVersionMessageHandler, _waitForCompInfoVersionTimeoutHandler);
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_MESSAGE,
+ _compInfoVersionCmdResultHandler,
+ MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+ COMP_METADATA_TYPE_VERSION);
+ }
+}
+
+void Vehicle::_compInfoVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
+{
+ if (result != MAV_RESULT_ACCEPTED) {
+ if (noResponsefromVehicle) {
+ qCDebug(VehicleLog) << "MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION no response from vehicle";
+ } else {
+ qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION error(%1)").arg(result);
+ }
+ vehicle->_waitForMavlinkMessageClear();
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
+ }
+}
+
+void Vehicle::_waitForCompInfoVersionMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
+{
+ qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION received";
+ vehicle->_componentInformation.componentInformationReceived(message);
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+void Vehicle::_waitForCompInfoVersionTimeoutHandler(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION timeout";
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
+}
+
+void Vehicle::_stateRequestCompInfoParam(Vehicle* vehicle)
+{
+ LinkInterface* link = vehicle->priorityLink();
+ if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+ qCDebug(VehicleLog) << "Skipping parameter component information request due to link type";
+ vehicle->_advanceInitialConnectStateMachine();
+ } else {
+ if (vehicle->_componentInformation.metaDataTypeSupported(COMP_METADATA_TYPE_PARAMETER)) {
+ qCDebug(VehicleLog) << "Requesting parameter component information";
+ vehicle->_waitForMavlinkMessage(MAVLINK_MSG_ID_COMPONENT_INFORMATION, 1000, _waitForCompInfoParamMessageHandler, _waitForCompInfoParamTimeoutHandler);
+ vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
+ MAV_CMD_REQUEST_MESSAGE,
+ _compInfoParamCmdResultHandler,
+ MAVLINK_MSG_ID_COMPONENT_INFORMATION,
+ COMP_METADATA_TYPE_PARAMETER);
+ } else {
+ qCDebug(VehicleLog) << "Skipping parameter component information request due to lack of support";
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
+ }
+ }
+}
+
+void Vehicle::_compInfoParamCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle)
+{
+ if (result != MAV_RESULT_ACCEPTED) {
+ if (noResponsefromVehicle) {
+ qCDebug(VehicleLog) << "MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER no response from vehicle";
+ } else {
+ qCDebug(VehicleLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER error(%1)").arg(result);
+ }
+ vehicle->_waitForMavlinkMessageClear();
+ vehicle->_advanceInitialConnectStateMachine(_stateRequestParameters);
+ }
+}
+
+void Vehicle::_waitForCompInfoParamMessageHandler(Vehicle* vehicle, const mavlink_message_t& message)
+{
+ qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER received";
+ vehicle->_componentInformation.componentInformationReceived(message);
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+void Vehicle::_waitForCompInfoParamTimeoutHandler(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER timeout";
+ vehicle->_advanceInitialConnectStateMachine();
+}
+
+void Vehicle::_stateRequestParameters(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "Requesting parameters";
+ vehicle->_parameterManager->refreshAllParameters();
+}
+
+void Vehicle::_stateRequestMission(Vehicle* vehicle)
+{
+ LinkInterface* link = vehicle->priorityLink();
+ if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) {
+ qCDebug(VehicleLog) << "Skipping first mission load request due to link type";
+ vehicle->_firstMissionLoadComplete();
+ } else {
+ qCDebug(VehicleLog) << "Requesting first mission load";
+ vehicle->_missionManager->loadFromVehicle();
+ }
+}
+
+void Vehicle::_stateRequestGeoFence(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "Requesting first geofence load";
+ if (vehicle->_geoFenceManager->supported()) {
+ vehicle->_geoFenceManager->loadFromVehicle();
+ } else {
+ qCDebug(VehicleLog) << "Geofence load skipped";
+ vehicle->_firstGeoFenceLoadComplete();
+ }
+}
+
+void Vehicle::_stateRequestRallyPoints(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "Requesting first rally point load";
+ if (vehicle->_rallyPointManager->supported()) {
+ vehicle->_rallyPointManager->loadFromVehicle();
+ } else {
+ qCDebug(VehicleLog) << "Rally Point load skipped";
+ vehicle->_firstRallyPointLoadComplete();
+ }
+}
+
+void Vehicle::_stateSignalInitialConnectComplete(Vehicle* vehicle)
+{
+ qCDebug(VehicleLog) << "Signalling initialConnectComplete";
+ emit vehicle->initialConnectComplete();
+}
+
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index d4e919fc64b60783ebd42c631a9d7d332ac04b69..d6b8e137a51034e889cb09f608829aaf7358f371 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -24,6 +24,7 @@
#include "SettingsFact.h"
#include "QGCMapCircle.h"
#include "TerrainFactGroup.h"
+#include "ComponentInformation.h"
class UAS;
class UASInterface;
@@ -1011,6 +1012,8 @@ public:
bool containsLink(LinkInterface* link) { return _links.contains(link); }
+ typedef void (*MavCmdResultHandler)(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+
/// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress
/// the command will be queued and sent when the previous command completes.
/// @param component Component to send to
@@ -1018,6 +1021,7 @@ public:
/// @param showError true: Display error to user if command failed, false: no error shown
/// Signals: mavCommandResult on success or failure
void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
+ void sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler errorHandler, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
void sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7);
/// Same as sendMavCommand but available from Qml.
@@ -1232,6 +1236,7 @@ signals:
void gimbalYawChanged ();
void gimbalDataChanged ();
void isROIEnabledChanged ();
+ void initialConnectComplete ();
private slots:
void _mavlinkMessageReceived (LinkInterface* link, mavlink_message_t message);
@@ -1250,9 +1255,9 @@ private slots:
/** @brief A new camera image has arrived */
void _imageReady (UASInterface* uas);
void _prearmErrorTimeout ();
- void _missionLoadComplete ();
- void _geoFenceLoadComplete ();
- void _rallyPointLoadComplete ();
+ void _firstMissionLoadComplete ();
+ void _firstGeoFenceLoadComplete ();
+ void _firstRallyPointLoadComplete ();
void _sendMavCommandAgain ();
void _clearCameraTriggerPoints ();
void _updateDistanceHeadingToHome ();
@@ -1266,7 +1271,6 @@ private slots:
void _trafficUpdate (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
void _orbitTelemetryTimeout ();
- void _protocolVersionTimeOut ();
void _updateFlightTime ();
private:
@@ -1289,8 +1293,6 @@ private:
void _handleExtendedSysState (mavlink_message_t& message);
void _handleCommandAck (mavlink_message_t& message);
void _handleCommandLong (mavlink_message_t& message);
- void _handleAutopilotVersion (LinkInterface* link, mavlink_message_t& message);
- void _handleProtocolVersion (LinkInterface* link, mavlink_message_t& message);
void _handleGpsRawInt (mavlink_message_t& message);
void _handleGlobalPositionInt (mavlink_message_t& message);
void _handleAltitude (mavlink_message_t& message);
@@ -1329,14 +1331,11 @@ private:
void _sendNextQueuedMavCommand ();
void _updatePriorityLink (bool updateActive, bool sendCommand);
void _commonInit ();
- void _startPlanRequest ();
void _setupAutoDisarmSignalling ();
void _setCapabilities (uint64_t capabilityBits);
void _updateArmed (bool armed);
bool _apmArmingNotRequired ();
void _pidTuningAdjustRates (bool setRatesForTuning);
- void _handleUnsupportedRequestAutopilotCapabilities();
- void _handleUnsupportedRequestProtocolVersion();
void _initializeCsv ();
void _writeCsvLine ();
void _flightTimerStart ();
@@ -1416,19 +1415,18 @@ private:
QGCCameraManager* _cameras;
typedef struct {
- int component;
- bool commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
- MAV_CMD command;
- MAV_FRAME frame;
- double rgParam[7];
- bool showError;
+ int component;
+ bool commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
+ MAV_CMD command;
+ MAV_FRAME frame;
+ double rgParam[7];
+ bool showError;
+ MavCmdResultHandler resultHandler;
} MavCommandQueueEntry_t;
QList _mavCommandQueue;
QTimer _mavCommandAckTimer;
int _mavCommandRetryCount;
- int _capabilitiesRetryCount = 0;
- QElapsedTimer _capabilitiesRetryElapsed;
static const int _mavCommandMaxRetryCount = 3;
static const int _mavCommandAckTimeoutMSecs = 3000;
static const int _mavCommandAckTimeoutMSecsHighLatency = 120000;
@@ -1444,13 +1442,8 @@ private:
bool _initialPlanRequestComplete;
MissionManager* _missionManager;
- bool _missionManagerInitialRequestSent;
-
GeoFenceManager* _geoFenceManager;
- bool _geoFenceManagerInitialRequestSent;
-
RallyPointManager* _rallyPointManager;
- bool _rallyPointManagerInitialRequestSent;
ParameterManager* _parameterManager = nullptr;
VehicleObjectAvoidance* _objectAvoidance = nullptr;
@@ -1551,6 +1544,54 @@ private:
QMap _chunkedStatusTextInfoMap;
QTimer _chunkedStatusTextTimer;
+ ComponentInformation _componentInformation;
+
+ typedef void (*WaitForMavlinkMessageTimeoutHandler)(Vehicle* vehicle);
+ typedef void (*WaitForMavlinkMessageMessageHandler)(Vehicle* vehicle, const mavlink_message_t& message);
+
+ /// Waits for the specified msecs for the message to be received. Calls timeoutHandler if not received.
+ void _waitForMavlinkMessage (int messageId, int timeoutMsecs, WaitForMavlinkMessageMessageHandler messageHandler, WaitForMavlinkMessageTimeoutHandler timeoutHandler);
+ void _waitForMavlinkMessageClear(void);
+
+ int _waitForMavlinkMessageId = 0;
+ int _waitForMavlinkMessageTimeoutMsecs = 0;
+ QElapsedTimer _waitForMavlinkMessageElapsed;
+ WaitForMavlinkMessageTimeoutHandler _waitForMavlinkMessageTimeoutHandler = nullptr;
+ WaitForMavlinkMessageMessageHandler _waitForMavlinkMessageMessageHandler = nullptr;
+
+ // Initial connection state machine
+ typedef void (*StateFn)(Vehicle* vehicle);
+ static const int _cInitialConnectStateEntries = 9;
+ static const StateFn _rgInitialConnectStates[_cInitialConnectStateEntries];
+ int _currentInitialConnectState = -1;
+ void _advanceInitialConnectStateMachine(void);
+ void _advanceInitialConnectStateMachine(StateFn stateFn);
+
+ static void _stateRequestCapabilities (Vehicle* vehicle);
+ static void _stateRequestProtocolVersion (Vehicle* vehicle);
+ static void _stateRequestCompInfoVersion (Vehicle* vehicle);
+ static void _stateRequestCompInfoParam (Vehicle* vehicle);
+ static void _stateRequestParameters (Vehicle* vehicle);
+ static void _stateRequestMission (Vehicle* vehicle);
+ static void _stateRequestGeoFence (Vehicle* vehicle);
+ static void _stateRequestRallyPoints (Vehicle* vehicle);
+ static void _stateSignalInitialConnectComplete (Vehicle* vehicle);
+
+ static void _capabilitiesCmdResultHandler (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+ static void _protocolVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+ static void _compInfoVersionCmdResultHandler(Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+ static void _compInfoParamCmdResultHandler (Vehicle* vehicle, MAV_RESULT result, bool noResponsefromVehicle);
+
+ static void _waitForAutopilotVersionMessageHandler (Vehicle* vehicle, const mavlink_message_t& message);
+ static void _waitForProtocolVersionMessageHandler (Vehicle* vehicle, const mavlink_message_t& message);
+ static void _waitForCompInfoVersionMessageHandler (Vehicle* vehicle, const mavlink_message_t& message);
+ static void _waitForCompInfoParamMessageHandler (Vehicle* vehicle, const mavlink_message_t& message);
+
+ static void _waitForAutopilotVersionTimeoutHandler (Vehicle* vehicle);
+ static void _waitForProtocolVersionTimeoutHandler (Vehicle* vehicle);
+ static void _waitForCompInfoVersionTimeoutHandler (Vehicle* vehicle);
+ static void _waitForCompInfoParamTimeoutHandler (Vehicle* vehicle);
+
// FactGroup facts
Fact _rollFact;
diff --git a/src/comm/MockLink.Version.MetaData.json b/src/comm/MockLink.Version.MetaData.json
new file mode 100644
index 0000000000000000000000000000000000000000..05c117e352f8c6d91db74936a82bf1ff8accbb27
--- /dev/null
+++ b/src/comm/MockLink.Version.MetaData.json
@@ -0,0 +1,8 @@
+{
+ "version": 1,
+ "vendorName": "QGC MockLink Vendor",
+ "modelName": "QGC MockLink Model",
+ "firmwareVersion": "1.0.0",
+ "hardwareVersion": "1.0.0",
+ "supportedCompMetadataTypes": [ 0, 1 ]
+}
\ No newline at end of file
diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index 0c560062ff990efce0de84ab1d98e4aa820ae930..1e1e7c712fbe846efe95b649563c8406c22f1c30 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -476,47 +476,36 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartBeat(msg);
break;
-
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
_handleParamRequestList(msg);
break;
-
case MAVLINK_MSG_ID_SET_MODE:
_handleSetMode(msg);
break;
-
case MAVLINK_MSG_ID_PARAM_SET:
_handleParamSet(msg);
break;
-
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
_handleParamRequestRead(msg);
break;
-
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
_handleFTP(msg);
break;
-
case MAVLINK_MSG_ID_COMMAND_LONG:
_handleCommandLong(msg);
break;
-
case MAVLINK_MSG_ID_MANUAL_CONTROL:
_handleManualControl(msg);
break;
-
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
_handleLogRequestList(msg);
break;
-
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
_handleLogRequestData(msg);
break;
-
case MAVLINK_MSG_ID_PARAM_MAP_RC:
_handleParamMapRC(msg);
break;
-
default:
break;
}
@@ -945,6 +934,11 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
commandResult = MAV_RESULT_ACCEPTED;
_respondWithAutopilotVersion();
break;
+ case MAV_CMD_REQUEST_MESSAGE:
+ if (request.param1 == MAVLINK_MSG_ID_COMPONENT_INFORMATION && _handleRequestMessage(request)) {
+ commandResult = MAV_RESULT_ACCEPTED;
+ }
+ break;
case MAV_CMD_USER_1:
// Test command which always returns MAV_RESULT_ACCEPTED
commandResult = MAV_RESULT_ACCEPTED;
@@ -1466,3 +1460,59 @@ void MockLink::_sendADSBVehicles(void)
respondWithMavlinkMessage(responseMsg);
}
+
+bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request)
+{
+ if (request.param1 != MAVLINK_MSG_ID_COMPONENT_INFORMATION) {
+ return false;
+ }
+
+ switch (static_cast(request.param2)) {
+ case COMP_METADATA_TYPE_VERSION:
+ _sendVersionMetaData();
+ return true;
+ case COMP_METADATA_TYPE_PARAMETER:
+ _sendParameterMetaData();
+ return true;
+ }
+
+ return false;
+}
+
+void MockLink::_sendVersionMetaData(void)
+{
+ mavlink_message_t responseMsg;
+ char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://version.json";
+ char translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = "";
+
+ mavlink_msg_component_information_pack_chan(_vehicleSystemId,
+ _vehicleComponentId,
+ _mavlinkChannel,
+ &responseMsg,
+ 0, // time_boot_ms
+ COMP_METADATA_TYPE_VERSION,
+ 1, // comp_metadata_uid
+ metaDataURI,
+ 0, // comp_translation_uid
+ translationURI);
+ respondWithMavlinkMessage(responseMsg);
+}
+
+void MockLink::_sendParameterMetaData(void)
+{
+ mavlink_message_t responseMsg;
+ char metaDataURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN] = "mavlinkftp://parameter.json";
+ char translationURI[MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN] = "";
+
+ mavlink_msg_component_information_pack_chan(_vehicleSystemId,
+ _vehicleComponentId,
+ _mavlinkChannel,
+ &responseMsg,
+ 0, // time_boot_ms
+ COMP_METADATA_TYPE_PARAMETER,
+ 1, // comp_metadata_uid
+ metaDataURI,
+ 0, // comp_translation_uid
+ translationURI);
+ respondWithMavlinkMessage(responseMsg);
+}
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index 8de34b67cef47b9e50f32963e6e2565c60f9562b..80831308e24c68f82a2679ef2bbc4c9e4d5e6b36 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -198,6 +198,7 @@ private:
void _handleLogRequestList (const mavlink_message_t& msg);
void _handleLogRequestData (const mavlink_message_t& msg);
void _handleParamMapRC (const mavlink_message_t& msg);
+ bool _handleRequestMessage (const mavlink_command_long_t& request);
float _floatUnionForParam (int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat);
void _sendHomePosition (void);
@@ -212,6 +213,8 @@ private:
void _logDownloadWorker (void);
void _sendADSBVehicles (void);
void _moveADSBVehicle (void);
+ void _sendVersionMetaData (void);
+ void _sendParameterMetaData (void);
static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
static MockLink* _startMockLink(MockConfiguration* mockConfig);
diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc
index 77427e593b0baefc4d2e91379b41fe918966861d..10d7e9379f2114fd230d7892811539eeaf3c0999 100644
--- a/src/qgcunittest/UnitTest.cc
+++ b/src/qgcunittest/UnitTest.cc
@@ -401,11 +401,9 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
_vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
QVERIFY(_vehicle);
- // Wait for plan request to complete
- if (!_vehicle->initialPlanRequestComplete()) {
- QSignalSpy spyPlan(_vehicle, SIGNAL(initialPlanRequestCompleteChanged(bool)));
- QCOMPARE(spyPlan.wait(10000), true);
- }
+ // Wait for initial connect sequence to complete
+ QSignalSpy spyPlan(_vehicle, SIGNAL(initialConnectComplete()));
+ QCOMPARE(spyPlan.wait(10000), true);
}
void UnitTest::_disconnectMockLink(void)