diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index e3c5bdf416c532e22285bb6dc178bff58b738483..2b22c5d24ae2ffa43d73580829d711868b44ba9d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -498,7 +498,9 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/qgcunittest/TCPLinkTest.h \ src/qgcunittest/TCPLoopBackServer.h \ src/qgcunittest/UnitTest.h \ - src/Vehicle/SendMavCommandTest.h \ + src/Vehicle/RequestMessageTest.h \ + src/Vehicle/SendMavCommandWithHandlerTest.h \ + src/Vehicle/SendMavCommandWithSignallingTest.h \ #src/qgcunittest/RadioConfigTest.h \ #src/AnalyzeView/LogDownloadTest.h \ #src/qgcunittest/FileDialogTest.h \ @@ -541,7 +543,9 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/qgcunittest/TCPLoopBackServer.cc \ src/qgcunittest/UnitTest.cc \ src/qgcunittest/UnitTestList.cc \ - src/Vehicle/SendMavCommandTest.cc \ + src/Vehicle/RequestMessageTest.cc \ + src/Vehicle/SendMavCommandWithHandlerTest.cc \ + src/Vehicle/SendMavCommandWithSignallingTest.cc \ #src/qgcunittest/RadioConfigTest.cc \ #src/AnalyzeView/LogDownloadTest.cc \ #src/qgcunittest/FileDialogTest.cc \ @@ -674,10 +678,13 @@ HEADERS += \ src/SHPFileHelper.h \ src/Terrain/TerrainQuery.h \ src/TerrainTile.h \ - src/Vehicle/ComponentInformation.h \ + src/Vehicle/ComponentInformationManager.h \ src/Vehicle/GPSRTKFactGroup.h \ + src/Vehicle/InitialConnectStateMachine.h \ + src/Vehicle/MAVLinkFTPManager.h \ src/Vehicle/MAVLinkLogManager.h \ src/Vehicle/MultiVehicleManager.h \ + src/Vehicle/StateMachine.h \ src/Vehicle/TerrainFactGroup.h \ src/Vehicle/TerrainProtocolHandler.h \ src/Vehicle/TrajectoryPoints.h \ @@ -887,10 +894,13 @@ SOURCES += \ src/SHPFileHelper.cc \ src/Terrain/TerrainQuery.cc \ src/TerrainTile.cc\ - src/Vehicle/ComponentInformation.cc \ + src/Vehicle/ComponentInformationManager.cc \ src/Vehicle/GPSRTKFactGroup.cc \ + src/Vehicle/InitialConnectStateMachine.cc \ + src/Vehicle/MAVLinkFTPManager.cc \ src/Vehicle/MAVLinkLogManager.cc \ src/Vehicle/MultiVehicleManager.cc \ + src/Vehicle/StateMachine.cc \ src/Vehicle/TerrainFactGroup.cc \ src/Vehicle/TerrainProtocolHandler.cc \ src/Vehicle/TrajectoryPoints.cc \ diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 4fca3c147b48c648ecd546b130ea5e5a276d4390..8cee9f19bd238421d8aa466302220d6394b17575 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -1005,6 +1005,8 @@ { "id": 500, "rawName": "MAV_CMD_START_RX_PAIR", "friendlyName": "Bind Spektrum receiver" }, { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" }, { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" }, + { "id": 512, "rawName": "MAV_CMD_REQUEST_MESSAGE" }, + { "id": 519, "rawName": "MAV_CMD_REQUEST_PROTOCOL_VERSION" }, { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" }, { "id": 530, diff --git a/src/Vehicle/ComponentInformation.cc b/src/Vehicle/ComponentInformation.cc deleted file mode 100644 index f1ee2f1a141e13527208db8dba9e85dfdaef06a3..0000000000000000000000000000000000000000 --- a/src/Vehicle/ComponentInformation.cc +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * (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 deleted file mode 100644 index 07147840b8993281c770812e11faa95afedc2a02..0000000000000000000000000000000000000000 --- a/src/Vehicle/ComponentInformation.h +++ /dev/null @@ -1,38 +0,0 @@ -/**************************************************************************** - * - * (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/ComponentInformationManager.cc b/src/Vehicle/ComponentInformationManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..7993c4304f7af6c2f7ef11eeb915e47ecd8d4625 --- /dev/null +++ b/src/Vehicle/ComponentInformationManager.cc @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * (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 "ComponentInformationManager.h" +#include "Vehicle.h" + +QGC_LOGGING_CATEGORY(ComponentInformationManagerLog, "ComponentInformationManagerLog") + +ComponentInformationManager::StateFn ComponentInformationManager::_rgStates[]= { + ComponentInformationManager::_stateRequestCompInfoVersion, + ComponentInformationManager::_stateRequestCompInfoParam, + ComponentInformationManager::_stateRequestAllCompInfoComplete +}; + +int ComponentInformationManager::_cStates = sizeof(ComponentInformationManager::_rgStates) / sizeof(ComponentInformationManager::_rgStates[0]); + +RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgStates[]= { + RequestMetaDataTypeStateMachine::_stateRequestCompInfo, +}; + +int RequestMetaDataTypeStateMachine::_cStates = sizeof(RequestMetaDataTypeStateMachine::_rgStates) / sizeof(RequestMetaDataTypeStateMachine::_rgStates[0]); + +ComponentInformationManager::ComponentInformationManager(Vehicle* vehicle) + : _vehicle (vehicle) + , _requestTypeStateMachine (this) +{ + +} + +int ComponentInformationManager::stateCount(void) const +{ + return _cStates; +} + +const ComponentInformationManager::StateFn* ComponentInformationManager::rgStates(void) const +{ + return &_rgStates[0]; +} + +void ComponentInformationManager::_componentInformationReceived(const mavlink_message_t& message) +{ + mavlink_component_information_t componentInformation; + mavlink_msg_component_information_decode(&message, &componentInformation); + + ComponentInformation_t* pCompInfo = nullptr; + switch (componentInformation.metadata_type) { + case COMP_METADATA_TYPE_VERSION: + qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_VERSION received"; + _versionCompInfoAvailable = true; + pCompInfo = &_versionCompInfo; + break; + case COMP_METADATA_TYPE_PARAMETER: + qCDebug(ComponentInformationManagerLog) << "COMPONENT_INFORMATION COMP_METADATA_TYPE_PARAMETER received"; + _paramCompInfoAvailable = true; + pCompInfo = &_parameterCompInfo; + break; + } + + if (pCompInfo) { + pCompInfo->metadataUID = componentInformation.metadata_uid; + pCompInfo->metadataURI = componentInformation.metadata_uri; + pCompInfo->translationUID = componentInformation.translation_uid; + pCompInfo->translationURI = componentInformation.translation_uri; + } +} + +void ComponentInformationManager::requestAllComponentInformation(RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData) +{ + _requestAllCompleteFn = requestAllCompletFn; + _requestAllCompleteFnData = requestAllCompleteFnData; + start(); +} + +void ComponentInformationManager::_stateRequestCompInfoVersion(StateMachine* stateMachine) +{ + ComponentInformationManager* compMgr = static_cast(stateMachine); + compMgr->_requestTypeStateMachine.request(COMP_METADATA_TYPE_VERSION); +} + +void ComponentInformationManager::_stateRequestCompInfoComplete(void) +{ + advance(); +} + +void ComponentInformationManager::_stateRequestCompInfoParam(StateMachine* stateMachine) +{ + ComponentInformationManager* compMgr = static_cast(stateMachine); + compMgr->_requestTypeStateMachine.request(COMP_METADATA_TYPE_PARAMETER); +} + +void ComponentInformationManager::_stateRequestAllCompInfoComplete(StateMachine* stateMachine) +{ + ComponentInformationManager* compMgr = static_cast(stateMachine); + (*compMgr->_requestAllCompleteFn)(compMgr->_requestAllCompleteFnData); + compMgr->_requestAllCompleteFn = nullptr; + compMgr->_requestAllCompleteFnData = nullptr; +} + +RequestMetaDataTypeStateMachine::RequestMetaDataTypeStateMachine(ComponentInformationManager* compMgr) + : _compMgr(compMgr) +{ + +} + +void RequestMetaDataTypeStateMachine::request(COMP_METADATA_TYPE type) +{ + _type = type; + _stateIndex = -1; + + start(); +} + +int RequestMetaDataTypeStateMachine::stateCount(void) const +{ + return _cStates; +} + +const RequestMetaDataTypeStateMachine::StateFn* RequestMetaDataTypeStateMachine::rgStates(void) const +{ + return &_rgStates[0]; +} + +void RequestMetaDataTypeStateMachine::statesCompleted(void) const +{ + _compMgr->_stateRequestCompInfoComplete(); +} + +QString RequestMetaDataTypeStateMachine::typeToString(void) +{ + return _type == COMP_METADATA_TYPE_VERSION ? "COMP_METADATA_TYPE_VERSION" : "COMP_METADATA_TYPE_PARAM"; +} + +static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t &message) +{ + RequestMetaDataTypeStateMachine* requestMachine = static_cast(resultHandlerData); + + if (result == MAV_RESULT_ACCEPTED) { + requestMachine->compMgr()->_componentInformationReceived(message); + } else { + switch (failureCode) { + case Vehicle::RequestMessageFailureCommandError: + qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 error(%2)").arg(requestMachine->typeToString()).arg(QGCMAVLink::mavResultToString(result)); + break; + case Vehicle::RequestMessageFailureCommandNotAcked: + qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 no response to command from vehicle").arg(requestMachine->typeToString()); + break; + case Vehicle::RequestMessageFailureMessageNotReceived: + qCDebug(ComponentInformationManagerLog) << QStringLiteral("MAV_CMD_REQUEST_MESSAGE COMPONENT_INFORMATION %1 vehicle did not send requested message").arg(requestMachine->typeToString()); + break; + default: + break; + } + } + requestMachine->advance(); +} + +void RequestMetaDataTypeStateMachine::_stateRequestCompInfo(StateMachine* stateMachine) +{ + RequestMetaDataTypeStateMachine* requestMachine = static_cast(stateMachine); + Vehicle* vehicle = requestMachine->_compMgr->vehicle(); + LinkInterface* link = vehicle->priorityLink(); + + if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) { + qCDebug(ComponentInformationManagerLog) << QStringLiteral("Skipping component information % 1 request due to link type").arg(requestMachine->typeToString()); + stateMachine->advance(); + } else { + qCDebug(ComponentInformationManagerLog) << "Requesting component information" << requestMachine->typeToString(); + vehicle->requestMessage( + _requestMessageResultHandler, + stateMachine, + MAV_COMP_ID_AUTOPILOT1, + MAVLINK_MSG_ID_COMPONENT_INFORMATION, + requestMachine->_type); + } +} + diff --git a/src/Vehicle/ComponentInformationManager.h b/src/Vehicle/ComponentInformationManager.h new file mode 100644 index 0000000000000000000000000000000000000000..6dae1d4752ccc76ace7bc9ed27baa558d7c3c299 --- /dev/null +++ b/src/Vehicle/ComponentInformationManager.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * (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 "StateMachine.h" + +Q_DECLARE_LOGGING_CATEGORY(ComponentInformationManagerLog) + +class Vehicle; +class ComponentInformationManager; + +class RequestMetaDataTypeStateMachine : public StateMachine +{ +public: + RequestMetaDataTypeStateMachine(ComponentInformationManager* compMgr); + + void request(COMP_METADATA_TYPE type); + ComponentInformationManager* compMgr(void) { return _compMgr; } + QString typeToString(void); + + // Overrides from StateMachine + int stateCount (void) const final; + const StateFn* rgStates (void) const final; + void statesCompleted (void) const final; + +private: + static void _stateRequestCompInfo (StateMachine* stateMachine); + static void _stateRequestMetaDataJson (StateMachine* stateMachine); + static void _stateRequestTranslationJson (StateMachine* stateMachine); + + ComponentInformationManager* _compMgr; + COMP_METADATA_TYPE _type = COMP_METADATA_TYPE_VERSION; + + static StateFn _rgStates[]; + static int _cStates; +}; + +class ComponentInformationManager : public StateMachine +{ +public: + ComponentInformationManager(Vehicle* vehicle); + + typedef struct { + uint32_t metadataUID; + QString metadataURI; + uint32_t translationUID; + QString translationURI; + } ComponentInformation_t; + + typedef void (*RequestAllCompleteFn)(void* requestAllCompleteFnData); + + void requestAllComponentInformation (RequestAllCompleteFn requestAllCompletFn, void * requestAllCompleteFnData); + Vehicle* vehicle (void) { return _vehicle; } + + // These methods should only be called by RequestMetaDataTypeStateMachine + void _componentInformationReceived(const mavlink_message_t& message); + void _stateRequestCompInfoComplete(void); + + // Overrides from StateMachine + int stateCount (void) const final; + const StateFn* rgStates (void) const final; + +private: + static void _stateRequestCompInfoVersion (StateMachine* stateMachine); + static void _stateRequestCompInfoParam (StateMachine* stateMachine); + static void _stateRequestAllCompInfoComplete (StateMachine* stateMachine); + + Vehicle* _vehicle = nullptr; + RequestMetaDataTypeStateMachine _requestTypeStateMachine; + bool _versionCompInfoAvailable = false; + ComponentInformation_t _versionCompInfo; + bool _paramCompInfoAvailable = false; + ComponentInformation_t _parameterCompInfo; + QList _supportedMetaDataTypes; + RequestAllCompleteFn _requestAllCompleteFn = nullptr; + void* _requestAllCompleteFnData = nullptr; + + static StateFn _rgStates[]; + static int _cStates; +}; diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc new file mode 100644 index 0000000000000000000000000000000000000000..0f14cfa56830b815e1f9058cf02cabb884cdd5fb --- /dev/null +++ b/src/Vehicle/InitialConnectStateMachine.cc @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * (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 "InitialConnectStateMachine.h" +#include "Vehicle.h" +#include "QGCCorePlugin.h" +#include "QGCOptions.h" +#include "FirmwarePlugin.h" +#include "ParameterManager.h" +#include "ComponentInformationManager.h" +#include "MissionManager.h" + +QGC_LOGGING_CATEGORY(InitialConnectStateMachineLog, "InitialConnectStateMachineLog") + +const StateMachine::StateFn InitialConnectStateMachine::_rgStates[] = { + InitialConnectStateMachine::_stateRequestCapabilities, + InitialConnectStateMachine::_stateRequestProtocolVersion, + InitialConnectStateMachine::_stateRequestCompInfo, + InitialConnectStateMachine::_stateRequestParameters, + InitialConnectStateMachine::_stateRequestMission, + InitialConnectStateMachine::_stateRequestGeoFence, + InitialConnectStateMachine::_stateRequestRallyPoints, + InitialConnectStateMachine::_stateSignalInitialConnectComplete +}; + +const int InitialConnectStateMachine::_cStates = sizeof(InitialConnectStateMachine::_rgStates) / sizeof(InitialConnectStateMachine::_rgStates[0]); + +InitialConnectStateMachine::InitialConnectStateMachine(Vehicle* vehicle) + : _vehicle(vehicle) +{ + +} + +int InitialConnectStateMachine::stateCount(void) const +{ + return _cStates; +} + +const InitialConnectStateMachine::StateFn* InitialConnectStateMachine::rgStates(void) const +{ + return &_rgStates[0]; +} + +void InitialConnectStateMachine::statesCompleted(void) const +{ + +} + +void InitialConnectStateMachine::_stateRequestCapabilities(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + LinkInterface* link = vehicle->priorityLink(); + if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) { + qCDebug(InitialConnectStateMachineLog) << "Skipping capability request due to link type"; + connectMachine->advance(); + } else { + qCDebug(InitialConnectStateMachineLog) << "Requesting capabilities"; + vehicle->_waitForMavlinkMessage(_waitForAutopilotVersionResultHandler, connectMachine, MAVLINK_MSG_ID_AUTOPILOT_VERSION, 1000); + vehicle->sendMavCommandWithHandler(_capabilitiesCmdResultHandler, + connectMachine, + MAV_COMP_ID_AUTOPILOT1, + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + 1); // Request firmware version + } +} + +void InitialConnectStateMachine::_capabilitiesCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle) +{ + InitialConnectStateMachine* connectMachine = static_cast(resultHandlerData); + Vehicle* vehicle = connectMachine->_vehicle; + + if (result != MAV_RESULT_ACCEPTED) { + if (noResponsefromVehicle) { + qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES no response from vehicle"; + } else { + qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES error(%1)").arg(result); + } + qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities"; + vehicle->_setCapabilities(0); + vehicle->_waitForMavlinkMessageClear(); + connectMachine->advance(); + } +} + +void InitialConnectStateMachine::_waitForAutopilotVersionResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message) +{ + InitialConnectStateMachine* connectMachine = static_cast(resultHandlerData); + Vehicle* vehicle = connectMachine->_vehicle; + + if (noResponsefromVehicle) { + qCDebug(InitialConnectStateMachineLog) << "AUTOPILOT_VERSION timeout"; + qCDebug(InitialConnectStateMachineLog) << "Setting no capabilities"; + vehicle->_setCapabilities(0); + } else { + qCDebug(InitialConnectStateMachineLog) << "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); + } + connectMachine->advance(); +} + +void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + LinkInterface* link = vehicle->priorityLink(); + + if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) { + qCDebug(InitialConnectStateMachineLog) << "Skipping protocol version request due to link type"; + connectMachine->advance(); + } else { + qCDebug(InitialConnectStateMachineLog) << "Requesting protocol version"; + vehicle->_waitForMavlinkMessage(_waitForProtocolVersionResultHandler, connectMachine, MAVLINK_MSG_ID_PROTOCOL_VERSION, 1000); + vehicle->sendMavCommandWithHandler(_protocolVersionCmdResultHandler, + connectMachine, + MAV_COMP_ID_AUTOPILOT1, + MAV_CMD_REQUEST_PROTOCOL_VERSION, + 1); // Request protocol version + } +} + +void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle) +{ + if (result != MAV_RESULT_ACCEPTED) { + InitialConnectStateMachine* connectMachine = static_cast(resultHandlerData); + Vehicle* vehicle = connectMachine->_vehicle; + + if (noResponsefromVehicle) { + qCDebug(InitialConnectStateMachineLog) << "MAV_CMD_REQUEST_PROTOCOL_VERSION no response from vehicle"; + } else { + qCDebug(InitialConnectStateMachineLog) << QStringLiteral("MAV_CMD_REQUEST_PROTOCOL_VERSION error(%1)").arg(result); + } + + // _mavlinkProtocolRequestMaxProtoVersion stays at 0 to indicate unknown + vehicle->_mavlinkProtocolRequestComplete = true; + vehicle->_setMaxProtoVersionFromBothSources(); + vehicle->_waitForMavlinkMessageClear(); + connectMachine->advance(); + } +} + +void InitialConnectStateMachine::_waitForProtocolVersionResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message) +{ + InitialConnectStateMachine* connectMachine = static_cast(resultHandlerData); + Vehicle* vehicle = connectMachine->_vehicle; + + if (noResponsefromVehicle) { + qCDebug(InitialConnectStateMachineLog) << "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(InitialConnectStateMachineLog) << QStringLiteral("Setting _maxProtoVersion to 100 due to timeout on receiving PROTOCOL_VERSION message."); + vehicle->_mavlinkProtocolRequestMaxProtoVersion = 100; + vehicle->_mavlinkProtocolRequestComplete = true; + vehicle->_setMaxProtoVersionFromBothSources(); + } else { + mavlink_protocol_version_t protoVersion; + mavlink_msg_protocol_version_decode(&message, &protoVersion); + + qCDebug(InitialConnectStateMachineLog) << "PROTOCOL_VERSION received mav_version:" << protoVersion.max_version; + vehicle->_mavlinkProtocolRequestMaxProtoVersion = protoVersion.max_version; + vehicle->_mavlinkProtocolRequestComplete = true; + vehicle->_setMaxProtoVersionFromBothSources(); + } + connectMachine->advance(); +} + +void InitialConnectStateMachine::_stateRequestCompInfo(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + qCDebug(InitialConnectStateMachineLog) << "_stateRequestCompInfo"; + vehicle->_componentInformationManager->requestAllComponentInformation(_stateRequestCompInfoComplete, connectMachine); +} + +void InitialConnectStateMachine::_stateRequestCompInfoComplete(void* requestAllCompleteFnData) +{ + InitialConnectStateMachine* connectMachine = static_cast(requestAllCompleteFnData); + + connectMachine->advance(); +} + +void InitialConnectStateMachine::_stateRequestParameters(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + qCDebug(InitialConnectStateMachineLog) << "_stateRequestParameters"; + vehicle->_parameterManager->refreshAllParameters(); +} + +void InitialConnectStateMachine::_stateRequestMission(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + LinkInterface* link = vehicle->priorityLink(); + + if (link->highLatency() || link->isPX4Flow() || link->isLogReplay()) { + qCDebug(InitialConnectStateMachineLog) << "_stateRequestMission: Skipping first mission load request due to link type"; + vehicle->_firstMissionLoadComplete(); + } else { + qCDebug(InitialConnectStateMachineLog) << "_stateRequestMission"; + vehicle->_missionManager->loadFromVehicle(); + } +} + +void InitialConnectStateMachine::_stateRequestGeoFence(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + qCDebug(InitialConnectStateMachineLog) << "_stateRequestGeoFence"; + if (vehicle->_geoFenceManager->supported()) { + vehicle->_geoFenceManager->loadFromVehicle(); + } else { + qCDebug(InitialConnectStateMachineLog) << "_stateRequestGeoFence: skipped due to no support"; + vehicle->_firstGeoFenceLoadComplete(); + } +} + +void InitialConnectStateMachine::_stateRequestRallyPoints(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + qCDebug(InitialConnectStateMachineLog) << "_stateRequestRallyPoints"; + if (vehicle->_rallyPointManager->supported()) { + vehicle->_rallyPointManager->loadFromVehicle(); + } else { + qCDebug(InitialConnectStateMachineLog) << "_stateRequestRallyPoints: skipping due to no support"; + vehicle->_firstRallyPointLoadComplete(); + } +} + +void InitialConnectStateMachine::_stateSignalInitialConnectComplete(StateMachine* stateMachine) +{ + InitialConnectStateMachine* connectMachine = static_cast(stateMachine); + Vehicle* vehicle = connectMachine->_vehicle; + + qCDebug(InitialConnectStateMachineLog) << "Signalling initialConnectComplete"; + emit vehicle->initialConnectComplete(); +} + diff --git a/src/Vehicle/InitialConnectStateMachine.h b/src/Vehicle/InitialConnectStateMachine.h new file mode 100644 index 0000000000000000000000000000000000000000..ab196c11a84ea16f3619506e3ac66c84ffd0335e --- /dev/null +++ b/src/Vehicle/InitialConnectStateMachine.h @@ -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. + * + ****************************************************************************/ + +#pragma once + +#include "StateMachine.h" +#include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_LOGGING_CATEGORY(InitialConnectStateMachineLog) + +class Vehicle; + +class InitialConnectStateMachine : public StateMachine +{ +public: + InitialConnectStateMachine(Vehicle* vehicle); + + // Overrides from StateMachine + int stateCount (void) const final; + const StateFn* rgStates (void) const final; + void statesCompleted (void) const final; + +private: + static void _stateRequestCapabilities (StateMachine* stateMachine); + static void _stateRequestProtocolVersion (StateMachine* stateMachine); + static void _stateRequestCompInfo (StateMachine* stateMachine); + static void _stateRequestCompInfoComplete (void* requestAllCompleteFnData); + static void _stateRequestParameters (StateMachine* stateMachine); + static void _stateRequestMission (StateMachine* stateMachine); + static void _stateRequestGeoFence (StateMachine* stateMachine); + static void _stateRequestRallyPoints (StateMachine* stateMachine); + static void _stateSignalInitialConnectComplete (StateMachine* stateMachine); + + static void _capabilitiesCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle); + static void _protocolVersionCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle); + + static void _waitForAutopilotVersionResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message); + static void _waitForProtocolVersionResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message); + + Vehicle* _vehicle; + + static const StateFn _rgStates[]; + static const int _cStates; +}; diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..c955470015303d45a1190d9e90b02b389ef93047 --- /dev/null +++ b/src/Vehicle/RequestMessageTest.cc @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * (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 "RequestMessageTest.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "MockLink.h" + +RequestMessageTest::TestCase_t RequestMessageTest::_rgTestCases[] = { + { MockLink::FailRequestMessageNone, MAV_RESULT_ACCEPTED, Vehicle::RequestMessageNoFailure, false }, + { MockLink::FailRequestMessageCommandAcceptedMsgNotSent, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureMessageNotReceived, false }, + { MockLink::FailRequestMessageCommandUnsupported, MAV_RESULT_UNSUPPORTED, Vehicle::RequestMessageFailureCommandError, false }, + { MockLink::FailRequestMessageCommandNoResponse, MAV_RESULT_FAILED, Vehicle::RequestMessageFailureCommandNotAcked, false }, + //{ MockLink::FailRequestMessageCommandAcceptedSecondAttempMsgSent, MAV_RESULT_FAILED, Vehicle::RequestMessageNoFailure, false }, +}; + +void RequestMessageTest::_requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message) +{ + TestCase_t* testCase = static_cast(resultHandlerData); + + testCase->resultHandlerCalled = true; + QCOMPARE((int)testCase->expectedCommandResult, (int)commandResult); + QCOMPARE((int)testCase->expectedFailureCode, (int)failureCode); + if (testCase->expectedFailureCode == Vehicle::RequestMessageNoFailure) { + QVERIFY(message.msgid == MAVLINK_MSG_ID_DEBUG); + } +} + +void RequestMessageTest::_testCaseWorker(TestCase_t& testCase) +{ + _connectMockLinkNoInitialConnectSequence(); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + + _mockLink->setRequestMessageFailureMode(testCase.failureMode); + vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_ALL, MAVLINK_MSG_ID_DEBUG); + for (int i=0; i<100; i++) { + QTest::qWait(100); + if (testCase.resultHandlerCalled) { + break; + } + } + QVERIFY(testCase.resultHandlerCalled); + + _disconnectMockLink(); +} + +void RequestMessageTest::_performTestCases(void) +{ + int index = 0; + for (TestCase_t& testCase: _rgTestCases) { + qDebug() << "Testing case" << index++; + _testCaseWorker(testCase); + } +} diff --git a/src/Vehicle/RequestMessageTest.h b/src/Vehicle/RequestMessageTest.h new file mode 100644 index 0000000000000000000000000000000000000000..af01f4a4549a764887ee03608e9aafc554db07d3 --- /dev/null +++ b/src/Vehicle/RequestMessageTest.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * (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 "UnitTest.h" +#include "MockLink.h" +#include "Vehicle.h" + +class RequestMessageTest : public UnitTest +{ + Q_OBJECT + +signals: + void resultHandlerCalled(void); + +private slots: + void _performTestCases(void); + +private: + typedef struct { + MockLink::RequestMessageFailureMode_t failureMode; + MAV_RESULT expectedCommandResult; + Vehicle::RequestMessageResultHandlerFailureCode_t expectedFailureCode; + bool resultHandlerCalled; + } TestCase_t; + + void _testCaseWorker(TestCase_t& testCase); + + static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); + + bool _resultHandlerCalled; + + static TestCase_t _rgTestCases[]; +}; diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.cc b/src/Vehicle/SendMavCommandWithHandlerTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..68e4e7eb4eed150aad3ea505ccb79aa0e87d86eb --- /dev/null +++ b/src/Vehicle/SendMavCommandWithHandlerTest.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 "SendMavCommandWithHandlerTest.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "MockLink.h" + +SendMavCommandWithHandlerTest::TestCase_t SendMavCommandWithHandlerTest::_rgTestCases[] = { + { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false }, + { MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, MAV_RESULT_ACCEPTED, false }, + { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED, false }, + { MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, MAV_RESULT_ACCEPTED, false }, + { MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, MAV_RESULT_ACCEPTED, false }, +}; + +void SendMavCommandWithHandlerTest::_mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle) +{ + TestCase_t* testCase = static_cast(resultHandlerData); + + QVERIFY(compId == MAV_COMP_ID_AUTOPILOT1); + QCOMPARE(testCase->expectedCommandResult, commandResult); + QCOMPARE(testCase->expectedNoResponseFromVehicle, noResponsefromVehicle); +} + +void SendMavCommandWithHandlerTest::_testCaseWorker(TestCase_t& testCase) +{ + _connectMockLinkNoInitialConnectSequence(); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + + vehicle->sendMavCommandWithHandler(_mavCmdResultHandler, &testCase, MAV_COMP_ID_ALL, testCase.command); + + _disconnectMockLink(); +} + +void SendMavCommandWithHandlerTest::_performTestCases(void) +{ + int index = 0; + for (TestCase_t& testCase: _rgTestCases) { + qDebug() << "Testing case" << index++; + _testCaseWorker(testCase); + } +} diff --git a/src/Vehicle/SendMavCommandWithHandlerTest.h b/src/Vehicle/SendMavCommandWithHandlerTest.h new file mode 100644 index 0000000000000000000000000000000000000000..99e8cb5921668b09ca43f9b6c49219c18de0fb49 --- /dev/null +++ b/src/Vehicle/SendMavCommandWithHandlerTest.h @@ -0,0 +1,33 @@ +/**************************************************************************** + * + * (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 "UnitTest.h" + +class SendMavCommandWithHandlerTest : public UnitTest +{ + Q_OBJECT + +private slots: + void _performTestCases(void); + +private: + typedef struct { + MAV_CMD command; + MAV_RESULT expectedCommandResult; + bool expectedNoResponseFromVehicle; + } TestCase_t; + + void _testCaseWorker(TestCase_t& testCase); + + static void _mavCmdResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle); + + static TestCase_t _rgTestCases[]; +}; diff --git a/src/Vehicle/SendMavCommandTest.cc b/src/Vehicle/SendMavCommandWithSignallingTest.cc similarity index 57% rename from src/Vehicle/SendMavCommandTest.cc rename to src/Vehicle/SendMavCommandWithSignallingTest.cc index a2d1ed1d4573d44a274061aacbe238ef68191706..3ff14a1c1d321a4d637e05e2801af69e61d13bd8 100644 --- a/src/Vehicle/SendMavCommandTest.cc +++ b/src/Vehicle/SendMavCommandWithSignallingTest.cc @@ -8,49 +8,49 @@ ****************************************************************************/ -#include "SendMavCommandTest.h" +#include "SendMavCommandWithSignallingTest.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" +#include "MockLink.h" -void SendMavCommandTest::_noFailure(void) +void SendMavCommandWithSignallingTest::_noFailure(void) { - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); - MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); - Vehicle* vehicle = vehicleMgr->activeVehicle(); - QVERIFY(vehicle); + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_1, true /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, true /* showError */); - QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(10000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_1); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED); QCOMPARE(arguments.at(4).toBool(), false); } -void SendMavCommandTest::_failureShowError(void) +void SendMavCommandWithSignallingTest::_failureShowError(void) { // Will pop error about request failure setExpectedMessageBox(QMessageBox::Ok); - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); QVERIFY(vehicle); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, true /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, true /* showError */); QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(10000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED); QCOMPARE(arguments.at(4).toBool(), false); @@ -58,65 +58,65 @@ void SendMavCommandTest::_failureShowError(void) checkExpectedMessageBox(); } -void SendMavCommandTest::_failureNoShowError(void) +void SendMavCommandWithSignallingTest::_failureNoShowError(void) { - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); QVERIFY(vehicle); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_2, false /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED, false /* showError */); QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(10000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_2); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED); QCOMPARE(arguments.at(4).toBool(), false); } -void SendMavCommandTest::_noFailureAfterRetry(void) +void SendMavCommandWithSignallingTest::_noFailureAfterRetry(void) { - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); QVERIFY(vehicle); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_3, true /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED, true /* showError */); QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(10000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_3); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_ACCEPTED); QCOMPARE(arguments.at(4).toBool(), false); } -void SendMavCommandTest::_failureAfterRetry(void) +void SendMavCommandWithSignallingTest::_failureAfterRetry(void) { // Will pop error about request failure setExpectedMessageBox(QMessageBox::Ok); - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); QVERIFY(vehicle); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_4, true /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED, true /* showError */); QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(10000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_4); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED); QCOMPARE(arguments.at(4).toBool(), false); @@ -124,28 +124,43 @@ void SendMavCommandTest::_failureAfterRetry(void) checkExpectedMessageBox(); } -void SendMavCommandTest::_failureAfterNoReponse(void) +void SendMavCommandWithSignallingTest::_failureAfterNoReponse(void) { // Will pop error about request failure setExpectedMessageBox(QMessageBox::Ok); - _connectMockLink(MAV_AUTOPILOT_ARDUPILOTMEGA); + _connectMockLinkNoInitialConnectSequence(); MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); Vehicle* vehicle = vehicleMgr->activeVehicle(); QVERIFY(vehicle); - vehicle->sendMavCommand(MAV_COMP_ID_ALL, MAV_CMD_USER_5, true /* showError */); + vehicle->sendMavCommand(MAV_COMP_ID_ALL, MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE, true /* showError */); QSignalSpy spyResult(vehicle, SIGNAL(mavCommandResult(int, int, int, int, bool))); QCOMPARE(spyResult.wait(20000), true); QList arguments = spyResult.takeFirst(); QCOMPARE(arguments.count(), 5); QCOMPARE(arguments.at(0).toInt(), vehicle->id()); - QCOMPARE(arguments.at(2).toInt(), (int)MAV_CMD_USER_5); + QCOMPARE(arguments.at(2).toInt(), (int)MockLink::MAV_CMD_MOCKLINK_NO_RESPONSE); QCOMPARE(arguments.at(3).toInt(), (int)MAV_RESULT_FAILED); QCOMPARE(arguments.at(4).toBool(), true); // User should have been notified checkExpectedMessageBox(); } + +void SendMavCommandWithSignallingTest::_unexpectedAck(void) +{ + _connectMockLinkNoInitialConnectSequence(); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + Vehicle* vehicle = vehicleMgr->activeVehicle(); + QSignalSpy spyResult(vehicle, &Vehicle::mavCommandResult); + + _mockLink->sendUnexpectedCommandAck(MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_ACCEPTED); + QCOMPARE(spyResult.wait(100), false); + + _mockLink->sendUnexpectedCommandAck(MockLink::MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED, MAV_RESULT_FAILED); + QCOMPARE(spyResult.wait(100), false); +} diff --git a/src/Vehicle/SendMavCommandTest.h b/src/Vehicle/SendMavCommandWithSignallingTest.h similarity index 55% rename from src/Vehicle/SendMavCommandTest.h rename to src/Vehicle/SendMavCommandWithSignallingTest.h index 851281e9a78ef4eba81a89c2988c843a79964699..48c23cc58eea28be15169b13165a7eac959b8bbd 100644 --- a/src/Vehicle/SendMavCommandTest.h +++ b/src/Vehicle/SendMavCommandWithSignallingTest.h @@ -7,25 +7,22 @@ * ****************************************************************************/ - -#ifndef SendMavCommandTest_H -#define SendMavCommandTest_H +#pragma once #include "UnitTest.h" -class SendMavCommandTest : public UnitTest +class SendMavCommandWithSignallingTest : public UnitTest { Q_OBJECT private slots: - void _noFailure(void); - void _failureShowError(void); - void _failureNoShowError(void); - void _noFailureAfterRetry(void); - void _failureAfterRetry(void); - void _failureAfterNoReponse(void); + void _noFailure (void); + void _failureShowError (void); + void _failureNoShowError (void); + void _noFailureAfterRetry (void); + void _failureAfterRetry (void); + void _failureAfterNoReponse (void); + void _unexpectedAck (void); private: }; - -#endif diff --git a/src/Vehicle/StateMachine.cc b/src/Vehicle/StateMachine.cc new file mode 100644 index 0000000000000000000000000000000000000000..a9df899359f42412366051b83832b95fe4cc7e1f --- /dev/null +++ b/src/Vehicle/StateMachine.cc @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * (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 "StateMachine.h" + +StateMachine::StateMachine(void) +{ + +} + +void StateMachine::start(void) +{ + _active = true; + advance(); +} + +void StateMachine::advance(void) +{ + if (_active) { + _stateIndex++; + if (_stateIndex < stateCount()) { + (*rgStates()[_stateIndex])(this); + } else { + _active = false; + statesCompleted(); + } + } +} + +void StateMachine::move(StateFn stateFn) +{ + if (_active) { + for (int i=0; i + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +class StateMachine +{ +public: + typedef void (*StateFn)(StateMachine* stateMachine); + + StateMachine(void); + + /// Start the state machine with the first step + void start(void); + + /// Advance the state machine to the next state and call the state function + void advance(void); + + /// Move the state machine to the specified state and call the state function + void move(StateFn stateFn); + + /// @return The number of states in the rgStates array + virtual int stateCount(void) const = 0; + + /// @return Array of states to execute + virtual const StateFn* rgStates(void) const = 0; + + /// Called when all states have completed + virtual void statesCompleted(void) const; + +protected: + bool _active = false; + int _stateIndex = -1; +}; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9b781de67abcee0a581254fcf8da7d93a8309fc1..c70a1830a4c62fd88992b333616e0128dc16b131 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -45,6 +45,9 @@ #include "QGCGeo.h" #include "TerrainProtocolHandler.h" #include "ParameterManager.h" +#include "MAVLinkFTPManager.h" +#include "ComponentInformationManager.h" +#include "InitialConnectStateMachine.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirspaceVehicleManager.h" @@ -93,18 +96,6 @@ 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, @@ -199,7 +190,6 @@ 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) @@ -277,7 +267,11 @@ Vehicle::Vehicle(LinkInterface* link, connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived, this, &Vehicle::_handletextMessageReceived); - _advanceInitialConnectStateMachine(); + // MAV_TYPE_GENERIC is used by unit test for creating a vehicle which doesn't do the connect sequence. This + // way we can test the methods that are used within the connect sequence. + if (!qgcApp()->runningUnitTests() || _vehicleType != MAV_TYPE_GENERIC) { + _initialConnectStateMachine->start(); + } _firmwarePlugin->initializeVehicle(this); for(auto& factName: factNames()) { @@ -386,7 +380,6 @@ 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) @@ -472,6 +465,10 @@ void Vehicle::_commonInit() _parameterManager = new ParameterManager(this); connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); + _componentInformationManager = new ComponentInformationManager(this); + _initialConnectStateMachine = new InitialConnectStateMachine(this); + _mavlinkFTPManager = new MAVLinkFTPManager(this); + _objectAvoidance = new VehicleObjectAvoidance(this, this); // GeoFenceManager needs to access ParameterManager so make sure to create after @@ -719,17 +716,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes return; } - 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); - } - } + _waitForMavlinkMessageMessageReceived(message); switch (message.msgid) { case MAVLINK_MSG_ID_HOME_POSITION: @@ -2573,13 +2560,13 @@ void Vehicle::_updateFlightTime() void Vehicle::_firstMissionLoadComplete() { disconnect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_firstMissionLoadComplete); - _advanceInitialConnectStateMachine(); + _initialConnectStateMachine->advance(); } void Vehicle::_firstGeoFenceLoadComplete() { disconnect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_firstGeoFenceLoadComplete); - _advanceInitialConnectStateMachine(); + _initialConnectStateMachine->advance(); } void Vehicle::_firstRallyPointLoadComplete() @@ -2587,19 +2574,21 @@ void Vehicle::_firstRallyPointLoadComplete() disconnect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_firstRallyPointLoadComplete); _initialPlanRequestComplete = true; emit initialPlanRequestCompleteChanged(true); - _advanceInitialConnectStateMachine(); + _initialConnectStateMachine->advance(); } void Vehicle::_parametersReady(bool parametersReady) { qDebug() << "_parametersReady" << parametersReady; + // Try to set current unix time to the vehicle _sendQGCTimeToVehicle(); // Send time twice, more likely to get to the vehicle on a noisy link _sendQGCTimeToVehicle(); if (parametersReady) { + disconnect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady); _setupAutoDisarmSignalling(); - _advanceInitialConnectStateMachine(); + _initialConnectStateMachine->advance(); } } @@ -3151,75 +3140,66 @@ void Vehicle::setCurrentMissionSequence(int seq) sendMessageOnLinkThreadSafe(priorityLink(), msg); } -void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, 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 = 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(); - } + _sendMavCommandWorker(false, // commandInt + false, // requestMessage + showError, + nullptr, // resultHandler + nullptr, // resultHandlerData + compId, + command, + MAV_FRAME_GLOBAL, + param1, param2, param3, param4, param5, param6, param7); } -void Vehicle::sendMavCommand(int component, MAV_CMD command, MavCmdResultHandler resultHandler, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +void Vehicle::sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void *resultHandlerData, int compId, MAV_CMD command, 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); - 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); + _sendMavCommandWorker(false, // commandInt + false, // requestMessage, + false, // showError + resultHandler, + resultHandlerData, + compId, + command, + MAV_FRAME_GLOBAL, + param1, param2, param3, param4, param5, param6, param7); +} - if (_mavCommandQueue.count() == 1) { - _mavCommandRetryCount = 0; - _sendMavCommandAgain(); - } +void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7) +{ + _sendMavCommandWorker(true, // commandInt + false, // requestMessage + showError, + nullptr, // resultHandler + nullptr, // resultHandlerData + compId, + command, + frame, + param1, param2, param3, param4, param5, param6, param7); } -void Vehicle::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) +void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { MavCommandQueueEntry_t entry; - entry.commandInt = true; - entry.component = component; - 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); - entry.rgParam[3] = static_cast(param4); - entry.rgParam[4] = param5; - entry.rgParam[5] = param6; - entry.rgParam[6] = static_cast(param7); - - _mavCommandQueue.append(entry); + entry.commandInt = commandInt; + entry.compId = compId; + entry.command = command; + entry.frame = frame; + entry.showError = showError; + entry.requestMessage = requestMessage; + entry.resultHandler = resultHandler; + entry.resultHandlerData = resultHandlerData; + entry.rgParam[0] = param1; + entry.rgParam[1] = param2; + entry.rgParam[2] = param3; + entry.rgParam[3] = param4; + entry.rgParam[4] = param5; + entry.rgParam[5] = param6; + entry.rgParam[6] = param7; + + _mavCommandQueue.enqueue(entry); if (_mavCommandQueue.count() == 1) { _mavCommandRetryCount = 0; @@ -3229,23 +3209,25 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, void Vehicle::_sendMavCommandAgain() { - if(!_mavCommandQueue.size()) { + if(_mavCommandQueue.isEmpty()) { qWarning() << "Command resend with no commands in queue"; _mavCommandAckTimer.stop(); return; } - MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0]; + MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0]; + QString rawCommandName =_toolbox->missionCommandTree()->rawName(queuedCommand.command); - if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { - emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); + if (_mavCommandRetryCount++ >= _mavCommandMaxRetryCount) { if (queuedCommand.resultHandler) { - (*queuedCommand.resultHandler)(this, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); + (*queuedCommand.resultHandler)(queuedCommand.resultHandlerData, queuedCommand.compId, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); + } else { + emit mavCommandResult(_id, queuedCommand.compId, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); } if (queuedCommand.showError) { - qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command))); + qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(rawCommandName)); } - _mavCommandQueue.removeFirst(); + _mavCommandQueue.dequeue(); _sendNextQueuedMavCommand(); return; } @@ -3256,12 +3238,17 @@ void Vehicle::_sendMavCommandAgain() // we aren't really sure whether they are correct or not. return; } - qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; + qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << rawCommandName << _mavCommandRetryCount; } _mavCommandAckTimer.start(); - qCDebug(VehicleLog) << "_sendMavCommandAgain sending" << queuedCommand.command; + if (queuedCommand.requestMessage) { + RequestMessageInfo_t* pInfo = static_cast(queuedCommand.resultHandlerData); + _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000); + } + + qCDebug(VehicleLog) << "_sendMavCommandAgain sending name:retry" << rawCommandName << _mavCommandRetryCount; mavlink_message_t msg; if (queuedCommand.commandInt) { @@ -3269,7 +3256,7 @@ void Vehicle::_sendMavCommandAgain() memset(&cmd, 0, sizeof(cmd)); cmd.target_system = _id; - cmd.target_component = queuedCommand.component; + cmd.target_component = queuedCommand.compId; cmd.command = queuedCommand.command; cmd.frame = queuedCommand.frame; cmd.param1 = queuedCommand.rgParam[0]; @@ -3289,7 +3276,7 @@ void Vehicle::_sendMavCommandAgain() memset(&cmd, 0, sizeof(cmd)); cmd.target_system = _id; - cmd.target_component = queuedCommand.component; + cmd.target_component = queuedCommand.compId; cmd.command = queuedCommand.command; cmd.confirmation = 0; cmd.param1 = queuedCommand.rgParam[0]; @@ -3311,7 +3298,7 @@ void Vehicle::_sendMavCommandAgain() void Vehicle::_sendNextQueuedMavCommand() { - if (_mavCommandQueue.count()) { + if (!_mavCommandQueue.isEmpty()) { _mavCommandRetryCount = 0; _sendMavCommandAgain(); } @@ -3322,7 +3309,8 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) 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); + QString rawCommandName =_toolbox->missionCommandTree()->rawName(static_cast(ack.command)); + qCDebug(VehicleLog) << QStringLiteral("_handleCommandAck command(%1) result(%2)").arg(rawCommandName).arg(QGCMAVLink::mavResultToString(static_cast(ack.result))); if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) { if (ack.result == MAV_RESULT_ACCEPTED) { @@ -3344,43 +3332,149 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) } #endif - bool showError = false; - MavCmdResultHandler resultHandler = nullptr; - if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { + if (!_mavCommandQueue.isEmpty() && ack.command == _mavCommandQueue.head().command) { + bool sendNextCommand = false; + MavCommandQueueEntry_t commandEntry = _mavCommandQueue.head(); _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 (commandEntry.requestMessage) { + RequestMessageInfo_t* pInfo = static_cast(commandEntry.resultHandlerData); + pInfo->commandAckReceived = true; + if (ack.result == MAV_RESULT_ACCEPTED) { + if (pInfo->messageReceived) { + delete pInfo; + sendNextCommand = true; + } else { + // We dont set sendNextCommand because we wait for the message wait to complete before sending next message + _waitForMavlinkMessageTimeoutActive = true; + _waitForMavlinkMessageElapsed.restart(); + } + } else { + sendNextCommand = true; + if (pInfo->messageReceived) { + qCWarning(VehicleLog) << "Internal Error: _handleCommandAck for requestMessage with result failure, but message already received"; + } else { + _waitForMavlinkMessageClear(); + (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast(ack.result), false /* noResponsefromVehicle */); + } + } + } else { + if (commandEntry.resultHandler) { + (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast(ack.result), false /* noResponsefromVehicle */); + } else { + if (commandEntry.showError) { + switch (ack.result) { + case MAV_RESULT_TEMPORARILY_REJECTED: + qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(rawCommandName)); + break; + case MAV_RESULT_DENIED: + qgcApp()->showAppMessage(tr("%1 command denied").arg(rawCommandName)); + break; + case MAV_RESULT_UNSUPPORTED: + qgcApp()->showAppMessage(tr("%1 command not supported").arg(rawCommandName)); + break; + case MAV_RESULT_FAILED: + qgcApp()->showAppMessage(tr("%1 command failed").arg(rawCommandName)); + break; + default: + // Do nothing + break; + } + } + emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */); + } + sendNextCommand = true; + } + + _mavCommandQueue.dequeue(); + if (sendNextCommand) { + _sendNextQueuedMavCommand(); + } + } else { + qCDebug(VehicleLog) << "_handleCommandAck Ack not in queue" << rawCommandName; } +} - if (showError) { - QString commandName = _toolbox->missionCommandTree()->friendlyName(static_cast(ack.command)); - switch (ack.result) { - case MAV_RESULT_TEMPORARILY_REJECTED: - qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(commandName)); - break; - case MAV_RESULT_DENIED: - qgcApp()->showAppMessage(tr("%1 command denied").arg(commandName)); - break; - case MAV_RESULT_UNSUPPORTED: - qgcApp()->showAppMessage(tr("%1 command not supported").arg(commandName)); - break; - case MAV_RESULT_FAILED: - qgcApp()->showAppMessage(tr("%1 command failed").arg(commandName)); - break; - default: - // Do nothing - break; +void Vehicle::_waitForMavlinkMessage(WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs) +{ + qCDebug(VehicleLog) << "_waitForMavlinkMessage msg:timeout" << messageId << timeoutMsecs; + _waitForMavlinkMessageResultHandler = resultHandler; + _waitForMavlinkMessageResultHandlerData = resultHandlerData; + _waitForMavlinkMessageId = messageId; + _waitForMavlinkMessageTimeoutActive = false; + _waitForMavlinkMessageTimeoutMsecs = timeoutMsecs; +} + +void Vehicle::_waitForMavlinkMessageClear(void) +{ + qCDebug(VehicleLog) << "_waitForMavlinkMessageClear"; + _waitForMavlinkMessageResultHandler = nullptr; + _waitForMavlinkMessageResultHandlerData = nullptr; + _waitForMavlinkMessageId = 0; +} + +void Vehicle::_waitForMavlinkMessageMessageReceived(const mavlink_message_t& message) +{ + if (_waitForMavlinkMessageId != 0) { + if (_waitForMavlinkMessageId == message.msgid) { + WaitForMavlinkMessageResultHandler resultHandler = _waitForMavlinkMessageResultHandler; + void* resultHandlerData = _waitForMavlinkMessageResultHandlerData; + qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message received" << _waitForMavlinkMessageId; + _waitForMavlinkMessageClear(); + (*resultHandler)(resultHandlerData, false /* noResponseFromVehicle */, message); + } else if (_waitForMavlinkMessageTimeoutActive && _waitForMavlinkMessageElapsed.elapsed() > _waitForMavlinkMessageTimeoutMsecs) { + WaitForMavlinkMessageResultHandler resultHandler = _waitForMavlinkMessageResultHandler; + void* resultHandlerData = _waitForMavlinkMessageResultHandlerData; + qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message timed out" << _waitForMavlinkMessageId; + _waitForMavlinkMessageClear(); + (*resultHandler)(resultHandlerData, true /* noResponseFromVehicle */, message); } } } +void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1, float param2, float param3, float param4, float param5) +{ + RequestMessageInfo_t* pInfo = new RequestMessageInfo_t; + + pInfo->msgId = messageId; + pInfo->compId = compId; + pInfo->resultHandler = resultHandler; + pInfo->resultHandlerData = resultHandlerData; + + _sendMavCommandWorker(false, // commandInt + true, // requestMessage, + false, // showError + _requestMessageCmdResultHandler, + pInfo, + compId, + MAV_CMD_REQUEST_MESSAGE, + MAV_FRAME_GLOBAL, + messageId, + param1, param2, param3, param4, param5, 0); +} + +void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle) +{ + RequestMessageInfo_t* pInfo = static_cast(resultHandlerData); + + pInfo->commandAckReceived = true; + if (result != MAV_RESULT_ACCEPTED) { + mavlink_message_t message; + (*pInfo->resultHandler)(pInfo->resultHandlerData, result, noResponsefromVehicle ? RequestMessageFailureCommandNotAcked : RequestMessageFailureCommandError, message); + } + if (pInfo->messageReceived) { + delete pInfo; + } +} + +void Vehicle::_requestMessageWaitForMessageResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message) +{ + RequestMessageInfo_t* pInfo = static_cast(resultHandlerData); + + pInfo->messageReceived = true; + (*pInfo->resultHandler)(pInfo->resultHandlerData, noResponsefromVehicle ? MAV_RESULT_FAILED : MAV_RESULT_ACCEPTED, noResponsefromVehicle ? RequestMessageFailureMessageNotReceived : RequestMessageNoFailure, message); +} + void Vehicle::setPrearmError(const QString& prearmError) { _prearmError = prearmError; @@ -4235,328 +4329,6 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo 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 d6b8e137a51034e889cb09f608829aaf7358f371..36124d1d07b9593c45bbfcfbf99e5ccacb468c71 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -14,6 +14,7 @@ #include #include #include +#include #include "FactGroup.h" #include "LinkInterface.h" @@ -24,7 +25,6 @@ #include "SettingsFact.h" #include "QGCMapCircle.h" #include "TerrainFactGroup.h" -#include "ComponentInformation.h" class UAS; class UASInterface; @@ -43,6 +43,9 @@ class Joystick; class VehicleObjectAvoidance; class TrajectoryPoints; class TerrainProtocolHandler; +class ComponentInformationManager; +class MAVLinkFTPManager; +class InitialConnectStateMachine; #if defined(QGC_AIRMAP_ENABLED) class AirspaceVehicleManager; @@ -1004,31 +1007,29 @@ public: void setConnectionLostEnabled(bool connectionLostEnabled); - ParameterManager* parameterManager() { return _parameterManager; } - ParameterManager* parameterManager() const { return _parameterManager; } - VehicleObjectAvoidance* objectAvoidance() { return _objectAvoidance; } + ParameterManager* parameterManager () { return _parameterManager; } + ParameterManager* parameterManager () const { return _parameterManager; } + MAVLinkFTPManager* mavlinkFTPManager () { return _mavlinkFTPManager; } + VehicleObjectAvoidance* objectAvoidance () { return _objectAvoidance; } static const int cMaxRcChannels = 18; 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 + /// @param compId Component to send to. /// @param command MAV_CMD to send /// @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); + void sendMavCommand(int compId, 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 sendMavCommandInt(int compId, 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. - Q_INVOKABLE void sendCommand(int component, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0) + Q_INVOKABLE void sendCommand(int compId, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0) { sendMavCommand( - component, static_cast(command), + compId, static_cast(command), showError, static_cast(param1), static_cast(param2), @@ -1039,6 +1040,36 @@ public: static_cast(param7)); } + /// Callback for sendMavCommandWithHandle + /// @param resultHandleData Opaque data passed in to sendMavCommand call + /// @param commandResult Ack result for command send + /// @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message + typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle); + + /// Sends the command and calls the callback with the result + /// @param resultHandler Callback for result, nullptr for no callback + /// @param resultHandleData Opaque data passed through callback + void sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, 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); + + typedef enum { + RequestMessageNoFailure, + RequestMessageFailureCommandError, + RequestMessageFailureCommandNotAcked, + RequestMessageFailureMessageNotReceived, + } RequestMessageResultHandlerFailureCode_t; + + /// Callback for requestMessage + /// @param resultHandlerData Opaque data which was passed in to requestMessage call + /// @param commandResult Result from ack to REQUEST_MESSAGE command + /// @param noResponseToCommand true: Vehicle never acked the REQUEST_MESSAGE command + /// @param messageNotReceived true: Vehicle never sent requested message + typedef void (*RequestMessageResultHandler)(void* resultHandlerData, MAV_RESULT commandResult, RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); + + /// Requests the vehicle to send the specified message. Will retry a number of times. + /// @param resultHandler Callback for result + /// @param resultHandlerData Opaque data passed back to resultHandler + void requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f); + int firmwareMajorVersion() const { return _firmwareMajorVersion; } int firmwareMinorVersion() const { return _firmwareMinorVersion; } int firmwarePatchVersion() const { return _firmwarePatchVersion; } @@ -1414,23 +1445,6 @@ 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; - MavCmdResultHandler resultHandler; - } MavCommandQueueEntry_t; - - QList _mavCommandQueue; - QTimer _mavCommandAckTimer; - int _mavCommandRetryCount; - static const int _mavCommandMaxRetryCount = 3; - static const int _mavCommandAckTimeoutMSecs = 3000; - static const int _mavCommandAckTimeoutMSecsHighLatency = 120000; - QString _prearmError; QTimer _prearmErrorTimer; static const int _prearmErrorTimeoutMSecs = 35 * 1000; ///< Take away prearm error after 35 seconds @@ -1445,8 +1459,11 @@ private: GeoFenceManager* _geoFenceManager; RallyPointManager* _rallyPointManager; - ParameterManager* _parameterManager = nullptr; - VehicleObjectAvoidance* _objectAvoidance = nullptr; + ParameterManager* _parameterManager = nullptr; + MAVLinkFTPManager* _mavlinkFTPManager = nullptr; + ComponentInformationManager* _componentInformationManager = nullptr; + InitialConnectStateMachine* _initialConnectStateMachine = nullptr; + VehicleObjectAvoidance* _objectAvoidance = nullptr; #if defined(QGC_AIRMAP_ENABLED) AirspaceVehicleManager* _airspaceVehicleManager; @@ -1544,53 +1561,58 @@ private: QMap _chunkedStatusTextInfoMap; QTimer _chunkedStatusTextTimer; - ComponentInformation _componentInformation; - - typedef void (*WaitForMavlinkMessageTimeoutHandler)(Vehicle* vehicle); - typedef void (*WaitForMavlinkMessageMessageHandler)(Vehicle* vehicle, const mavlink_message_t& message); + /// Callback for waitForMavlinkMessage + /// @param resultHandleData Opaque data passed in to waitForMavlinkMessage call + /// @param commandResult Ack result for command send + /// @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message + typedef void (*WaitForMavlinkMessageResultHandler)(void* resultHandlerData, bool noResponsefromVehicle, 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 _waitForMavlinkMessage (WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs); void _waitForMavlinkMessageClear(void); - int _waitForMavlinkMessageId = 0; - int _waitForMavlinkMessageTimeoutMsecs = 0; + int _waitForMavlinkMessageId = 0; + bool _waitForMavlinkMessageTimeoutActive = false; + 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); + WaitForMavlinkMessageResultHandler _waitForMavlinkMessageResultHandler = nullptr; + void* _waitForMavlinkMessageResultHandlerData = nullptr; + + void _waitForMavlinkMessageMessageReceived(const mavlink_message_t& message); + + // requestMessage handling + typedef struct { + bool commandAckReceived = false; // We keep track of the ack/message being received since the order in which this will come in is random + bool messageReceived = false; // We only delete the allocated RequestMessageInfo_t when both happen (or the message wait times out) + int msgId = 0; + int compId = 0; + RequestMessageResultHandler resultHandler = nullptr; + void* resultHandlerData = nullptr; + } RequestMessageInfo_t; + + static void _requestMessageCmdResultHandler (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle); + static void _requestMessageWaitForMessageResultHandler (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message); + + typedef struct { + int compId; + bool commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG + MAV_CMD command; + MAV_FRAME frame; + float rgParam[7]; + bool showError; + bool requestMessage; // true: this is from a requestMessage call + MavCmdResultHandler resultHandler; + void* resultHandlerData; + } MavCommandQueueEntry_t; + + QQueue _mavCommandQueue; + QTimer _mavCommandAckTimer; + int _mavCommandRetryCount; + static const int _mavCommandMaxRetryCount = 3; + static const int _mavCommandAckTimeoutMSecs = 3000; + static const int _mavCommandAckTimeoutMSecsHighLatency = 120000; + + void _sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7); // FactGroup facts @@ -1666,4 +1688,6 @@ private: // Settings keys static const char* _settingsGroup; static const char* _joystickEnabledSettingsKey; + + friend class InitialConnectStateMachine; }; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1e1e7c712fbe846efe95b649563c8406c22f1c30..41b9043e2fa9030e98a884589dc0c630d2c9603a 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -367,18 +367,18 @@ void MockLink::_sendSysStatus(void) } mavlink_message_t msg; mavlink_msg_sys_status_pack_chan( - _vehicleSystemId, - _vehicleComponentId, - static_cast(_mavlinkChannel), - &msg, - 0, // onboard_control_sensors_present - 0, // onboard_control_sensors_enabled - 0, // onboard_control_sensors_health - 250, // load - 4200 * 4, // voltage_battery - 8000, // current_battery - _batteryRemaining, // battery_remaining - 0,0,0,0,0,0); + _vehicleSystemId, + _vehicleComponentId, + static_cast(_mavlinkChannel), + &msg, + 0, // onboard_control_sensors_present + 0, // onboard_control_sensors_enabled + 0, // onboard_control_sensors_health + 250, // load + 4200 * 4, // voltage_battery + 8000, // current_battery + _batteryRemaining, // battery_remaining + 0,0,0,0,0,0); respondWithMavlinkMessage(msg); } @@ -429,7 +429,6 @@ void MockLink::_writeBytesQueued(const QByteArray bytes) _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count()); } - } /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell @@ -909,6 +908,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) static bool firstCmdUser3 = true; static bool firstCmdUser4 = true; + bool noAck = false; mavlink_command_long_t request; uint8_t commandResult = MAV_RESULT_UNSUPPORTED; @@ -935,19 +935,22 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) _respondWithAutopilotVersion(); break; case MAV_CMD_REQUEST_MESSAGE: - if (request.param1 == MAVLINK_MSG_ID_COMPONENT_INFORMATION && _handleRequestMessage(request)) { + if (_handleRequestMessage(request, noAck)) { + if (noAck) { + return; + } commandResult = MAV_RESULT_ACCEPTED; } break; - case MAV_CMD_USER_1: + case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED: // Test command which always returns MAV_RESULT_ACCEPTED commandResult = MAV_RESULT_ACCEPTED; break; - case MAV_CMD_USER_2: + case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED: // Test command which always returns MAV_RESULT_FAILED commandResult = MAV_RESULT_FAILED; break; - case MAV_CMD_USER_3: + case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED: // Test command which returns MAV_RESULT_ACCEPTED on second attempt if (firstCmdUser3) { firstCmdUser3 = false; @@ -957,7 +960,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) commandResult = MAV_RESULT_ACCEPTED; } break; - case MAV_CMD_USER_4: + case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED: // Test command which returns MAV_RESULT_FAILED on second attempt if (firstCmdUser4) { firstCmdUser4 = false; @@ -967,7 +970,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) commandResult = MAV_RESULT_FAILED; } break; - case MAV_CMD_USER_5: + case MAV_CMD_MOCKLINK_NO_RESPONSE: // No response return; break; @@ -987,6 +990,22 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) respondWithMavlinkMessage(commandAck); } +void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult) +{ + mavlink_message_t commandAck; + mavlink_msg_command_ack_pack_chan(_vehicleSystemId, + _vehicleComponentId, + _mavlinkChannel, + &commandAck, + command, + ackResult, + 0, // progress + 0, // result_param2 + 0, // target_system + 0); // target_component + respondWithMavlinkMessage(commandAck); +} + void MockLink::_respondWithAutopilotVersion(void) { mavlink_message_t msg; @@ -1147,7 +1166,7 @@ void MockLink::_sendStatusTextMessages(void) { MAV_SEVERITY_NOTICE, "Status text notice" }, { MAV_SEVERITY_INFO, "Status text info" }, { MAV_SEVERITY_DEBUG, "Status text debug" }, - }; +}; mavlink_message_t msg; @@ -1276,6 +1295,11 @@ MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } +MockLink* MockLink::startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +{ + return _startMockLinkWorker("No Initial Connect MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode); +} + MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); @@ -1461,18 +1485,44 @@ void MockLink::_sendADSBVehicles(void) respondWithMavlinkMessage(responseMsg); } -bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request) +bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request, bool& noAck) { - if (request.param1 != MAVLINK_MSG_ID_COMPONENT_INFORMATION) { - return false; + noAck = false; + + switch ((int)request.param1) { + case MAVLINK_MSG_ID_COMPONENT_INFORMATION: + switch (static_cast(request.param2)) { + case COMP_METADATA_TYPE_VERSION: + _sendVersionMetaData(); + return true; + case COMP_METADATA_TYPE_PARAMETER: + _sendParameterMetaData(); + return true; + } + break; + case MAVLINK_MSG_ID_DEBUG: + switch (_requestMessageFailureMode) { + case FailRequestMessageNone: + break; + case FailRequestMessageCommandAcceptedMsgNotSent: + return true; + case FailRequestMessageCommandUnsupported: + return false; + case FailRequestMessageCommandNoResponse: + noAck = true; + return true; + case FailRequestMessageCommandAcceptedSecondAttempMsgSent: + return true; + } + { + mavlink_message_t responseMsg; + mavlink_msg_debug_pack_chan(_vehicleSystemId, + _vehicleComponentId, + _mavlinkChannel, + &responseMsg, + 0, 0, 0); + respondWithMavlinkMessage(responseMsg); } - - switch (static_cast(request.param2)) { - case COMP_METADATA_TYPE_VERSION: - _sendVersionMetaData(); - return true; - case COMP_METADATA_TYPE_PARAMETER: - _sendParameterMetaData(); return true; } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 80831308e24c68f82a2679ef2bbc4c9e4d5e6b36..226e417eda1460e379f0776c65e2a8aeef478a3b 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -121,12 +121,11 @@ public: MockLinkFileServer* getFileServer(void) { return _fileServer; } - // Virtuals from LinkInterface - virtual QString getName(void) const { return _name; } - virtual void requestReset(void){ } - virtual bool isConnected(void) const { return _connected; } - virtual qint64 getConnectionSpeed(void) const { return 100000000; } - virtual qint64 bytesAvailable(void) { return 0; } + // Overrides from LinkInterface + QString getName (void) const override { return _name; } + void requestReset (void) override { } + bool isConnected (void) const override { return _connected; } + qint64 getConnectionSpeed (void) const override { return 100000000; } // These are left unimplemented in order to cause linker errors which indicate incorrect usage of // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. @@ -147,18 +146,38 @@ public: /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } + void sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult); + /// Reset the state of the MissionItemHandler to no items, no transactions in progress. void resetMissionItemHandler(void) { _missionItemHandler.reset(); } /// Returns the filename for the simulated log file. Only available after a download is requested. QString logDownloadFile(void) { return _logDownloadFilename; } - static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startGenericMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startGenericMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startNoInitialConnectMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + + // Special commands for testing COMMAND_LONG handlers + static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1; + static const MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2; + static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3; + static const MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4; + static const MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5; + + // Special message ids for testing requestMessage support + typedef enum { + FailRequestMessageNone, + FailRequestMessageCommandAcceptedMsgNotSent, + FailRequestMessageCommandUnsupported, + FailRequestMessageCommandNoResponse, + FailRequestMessageCommandAcceptedSecondAttempMsgSent, + } RequestMessageFailureMode_t; + void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; } signals: void writeBytesQueuedSignal(const QByteArray bytes); @@ -174,11 +193,11 @@ private slots: private: // From LinkInterface - virtual bool _connect(void); - virtual void _disconnect(void); + bool _connect(void) final; + void _disconnect(void) final; // QThread override - virtual void run(void); + void run(void) final; // MockLink methods void _sendHeartBeat (void); @@ -198,7 +217,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); + bool _handleRequestMessage (const mavlink_command_long_t& request, bool& noAck); float _floatUnionForParam (int componentId, const QString& paramName); void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat); void _sendHomePosition (void); @@ -269,6 +288,8 @@ private: QGeoCoordinate _adsbVehicleCoordinate; double _adsbAngle; + RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone; + static double _defaultVehicleLatitude; static double _defaultVehicleLongitude; static double _defaultVehicleAltitude; diff --git a/src/comm/MockLinkFileServer.cc b/src/comm/MockLinkFileServer.cc index 15b727b3e348b84f1cf545e5a2e0a61ef3172a87..a72abd871abf0e8a2d6b3c3f16c6a29ceca0df7a 100644 --- a/src/comm/MockLinkFileServer.cc +++ b/src/comm/MockLinkFileServer.cc @@ -135,6 +135,13 @@ void MockLinkFileServer::_openCommand(uint8_t senderSystemId, uint8_t senderComp break; } } + + if (!found) { + if (path == "/version.json") { + qDebug() << "Requesting version json"; + } + } + if (!found) { _sendNak(senderSystemId, senderComponentId, FileManager::kErrFail, outgoingSeqNumber, FileManager::kCmdOpenFileRO); return; diff --git a/src/comm/QGCMAVLink.cc b/src/comm/QGCMAVLink.cc index a6ea163e617229256b685b18de5aee1098e4b75a..c791df4e67b6b9bc9d4b73438d072c2cc3a78bf9 100644 --- a/src/comm/QGCMAVLink.cc +++ b/src/comm/QGCMAVLink.cc @@ -60,3 +60,23 @@ bool QGCMAVLink::isVTOL(MAV_TYPE mavType) return false; } } + +QString QGCMAVLink::mavResultToString(MAV_RESULT result) +{ + switch (result) { + case MAV_RESULT_ACCEPTED: + return QStringLiteral("MAV_RESULT_ACCEPTED"); + case MAV_RESULT_TEMPORARILY_REJECTED: + return QStringLiteral("MAV_RESULT_TEMPORARILY_REJECTED"); + case MAV_RESULT_DENIED: + return QStringLiteral("MAV_RESULT_DENIED"); + case MAV_RESULT_UNSUPPORTED: + return QStringLiteral("MAV_RESULT_UNSUPPORTED"); + case MAV_RESULT_FAILED: + return QStringLiteral("MAV_RESULT_FAILED"); + case MAV_RESULT_IN_PROGRESS: + return QStringLiteral("MAV_RESULT_IN_PROGRESS"); + default: + return QStringLiteral("MAV_RESULT unknown %1").arg(result); + } +} diff --git a/src/comm/QGCMAVLink.h b/src/comm/QGCMAVLink.h index 19f710bd216f3d644c272bb46539b23bee3c0426..e23eea4836e57fee55b67acff14a21430b92b6e4 100644 --- a/src/comm/QGCMAVLink.h +++ b/src/comm/QGCMAVLink.h @@ -7,15 +7,10 @@ * ****************************************************************************/ - -/** - * @file - * @brief MAVLink header file for QGroundControl - * @author Lorenz Meier - */ - #pragma once +#include + #define MAVLINK_USE_MESSAGE_INFO #define MAVLINK_EXTERNAL_RX_STATUS // Single m_mavlink_status instance is in QGCApplication.cc #include // Hack workaround for Mav 2.0 header problem with respect to offsetof usage @@ -47,11 +42,12 @@ extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; class QGCMAVLink { public: - static bool isFixedWing(MAV_TYPE mavType); - static bool isRover(MAV_TYPE mavType); - static bool isSub(MAV_TYPE mavType); - static bool isMultiRotor(MAV_TYPE mavType); - static bool isVTOL(MAV_TYPE mavType); + static bool isFixedWing (MAV_TYPE mavType); + static bool isRover (MAV_TYPE mavType); + static bool isSub (MAV_TYPE mavType); + static bool isMultiRotor (MAV_TYPE mavType); + static bool isVTOL (MAV_TYPE mavType); + static QString mavResultToString (MAV_RESULT result); }; diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index 10d7e9379f2114fd230d7892811539eeaf3c0999..23a94d2509664659e294f2b987a958d9bec6739f 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -389,21 +389,25 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot) case MAV_AUTOPILOT_GENERIC: _mockLink = MockLink::startGenericMockLink(false); break; + case MAV_AUTOPILOT_INVALID: + _mockLink = MockLink::startNoInitialConnectMockLink(false); + break; default: qWarning() << "Type not supported"; break; } // Wait for the Vehicle to get created - QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged); QCOMPARE(spyVehicle.wait(10000), true); - QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable()); _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); QVERIFY(_vehicle); - // Wait for initial connect sequence to complete - QSignalSpy spyPlan(_vehicle, SIGNAL(initialConnectComplete())); - QCOMPARE(spyPlan.wait(10000), true); + if (autopilot != MAV_AUTOPILOT_INVALID) { + // Wait for initial connect sequence to complete + QSignalSpy spyPlan(_vehicle, &Vehicle::initialConnectComplete); + QCOMPARE(spyPlan.wait(30000), true); + } } void UnitTest::_disconnectMockLink(void) diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index c443a3f2dae16431f24d6a315aa5bd0e28dc7d50..5a10245b2700d0aae11343cf43cd7bc59c24d2da 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -120,6 +120,7 @@ protected slots: protected: void _connectMockLink(MAV_AUTOPILOT autopilot = MAV_AUTOPILOT_PX4); + void _connectMockLinkNoInitialConnectSequence(void) { _connectMockLink(MAV_AUTOPILOT_INVALID); } void _disconnectMockLink(void); void _createMainWindow(void); void _closeMainWindow(bool cancelExpected = false); diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 5fa6edd03852d8fd58cf988ce6c3efc27119963b..b915e544ea985b2bb9e8c27bf537e352a3aeff9d 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -30,7 +30,8 @@ #include "ParameterManagerTest.h" #include "MissionCommandTreeTest.h" //#include "LogDownloadTest.h" -#include "SendMavCommandTest.h" +#include "SendMavCommandWithSignallingTest.h" +#include "SendMavCommandWithHandlerTest.h" #include "VisualMissionItemTest.h" #include "CameraSectionTest.h" #include "SpeedSectionTest.h" @@ -44,6 +45,7 @@ #include "TransectStyleComplexItemTest.h" #include "CameraCalcTest.h" #include "FWLandingPatternTest.h" +#include "RequestMessageTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -51,6 +53,9 @@ UT_REGISTER_TEST(FactSystemTestPX4) UT_REGISTER_TEST(GeoTest) UT_REGISTER_TEST(LinkManagerTest) //UT_REGISTER_TEST(MessageBoxTest) +UT_REGISTER_TEST(SendMavCommandWithSignallingTest) +UT_REGISTER_TEST(SendMavCommandWithHandlerTest) +UT_REGISTER_TEST(RequestMessageTest) UT_REGISTER_TEST(MissionItemTest) UT_REGISTER_TEST(SimpleMissionItemTest) UT_REGISTER_TEST(MissionControllerTest) @@ -61,7 +66,6 @@ UT_REGISTER_TEST(TCPLinkTest) UT_REGISTER_TEST(ParameterManagerTest) UT_REGISTER_TEST(MissionCommandTreeTest) //UT_REGISTER_TEST(LogDownloadTest) -UT_REGISTER_TEST(SendMavCommandTest) UT_REGISTER_TEST(SurveyComplexItemTest) UT_REGISTER_TEST(CameraSectionTest) UT_REGISTER_TEST(SpeedSectionTest)