diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index 03b994d2409ee873d2a9a7da5ef7aeb048f32cd9..0d223761eb9ca126439bd36e2a5d719064514a15 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -81,7 +81,17 @@ ParameterManager::ParameterManager(Vehicle* vehicle) // Ensure the cache directory exists QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache"); - refreshAllParameters(); + if (_vehicle->highLatencyLink()) { + // High latency links don't load parameters + _parametersReady = true; + _missingParameters = true; + _initialLoadComplete = true; + _waitingForDefaultComponent = false; + emit parametersReadyChanged(_parametersReady); + emit missingParametersChanged(_missingParameters); + } else { + refreshAllParameters(); + } } ParameterManager::~ParameterManager() @@ -133,12 +143,12 @@ void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString } #endif - if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") { - if (!_initialLoadComplete && !_logReplay) { - /* we received a cache hash, potentially load from cache */ - _tryCacheHashLoad(vehicleId, componentId, value); - } - return; + if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") { + if (!_initialLoadComplete && !_logReplay) { + /* we received a cache hash, potentially load from cache */ + _tryCacheHashLoad(vehicleId, componentId, value); + } + return; } _initialRequestTimeoutTimer.stop(); diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc index a2041f935c4c1268df122af79f6a5739b66d3506..7896e4c7c8b93fd593cfb47947a31dad31b5a580 100644 --- a/src/MissionManager/PlanMasterController.cc +++ b/src/MissionManager/PlanMasterController.cc @@ -157,6 +157,11 @@ void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle) void PlanMasterController::loadFromVehicle(void) { + if (_managerVehicle->highLatencyLink()) { + qgcApp()->showMessage(tr("Download not supported on high latency links.")); + return; + } + if (offline()) { qCWarning(PlanMasterControllerLog) << "PlanMasterController::loadFromVehicle called while offline"; } else if (!_editMode) { @@ -256,6 +261,11 @@ void PlanMasterController::_sendRallyPointsComplete(void) void PlanMasterController::sendToVehicle(void) { + if (_managerVehicle->highLatencyLink()) { + qgcApp()->showMessage(tr("Upload not supported on high latency links.")); + return; + } + if (offline()) { qCWarning(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle called while offline"; } else if (syncInProgress()) { diff --git a/src/MissionManager/VisualMissionItem.cc b/src/MissionManager/VisualMissionItem.cc index 1f0173f156851138b55adc3df8c39b93655f22bf..ba309b72f6ac54b434f2ca2d5aca0f85eab7d591 100644 --- a/src/MissionManager/VisualMissionItem.cc +++ b/src/MissionManager/VisualMissionItem.cc @@ -63,7 +63,7 @@ VisualMissionItem::VisualMissionItem(const VisualMissionItem& other, QObject* pa { *this = other; - // Don't get terrain altitude information for submarines or boards + // Don't get terrain altitude information for submarines or boats if (_vehicle->vehicleType() != MAV_TYPE_SUBMARINE && _vehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) { connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude); } diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 86e632823f89c065d441673f2a91fbbbeff73c94..5fa0dceda761c04108a6d2d0a36cc53a54885cd7 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -72,7 +72,7 @@ void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) this); } -void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType) +void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) { if (componentId != MAV_COMP_ID_AUTOPILOT1) { // Special case for PX4 Flow @@ -82,7 +82,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle << link->getName() << vehicleId << componentId - << vehicleMavlinkVersion << vehicleFirmwareType << vehicleType; return; @@ -108,11 +107,10 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle break; } - qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType " + qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleFirmwareType:vehicleType " << link->getName() << vehicleId << componentId - << vehicleMavlinkVersion << vehicleFirmwareType << vehicleType; @@ -120,16 +118,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle _app->showMessage(tr("Warning: A vehicle is using the same system id as %1: %2").arg(qgcApp()->applicationName()).arg(vehicleId)); } - // QSettings settings; - // bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool(); - // if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) { - // _ignoreVehicleIds += vehicleId; - // _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and %2 differ! " - // "It is unsafe to use different MAVLink versions. " - // "%2 therefore refuses to connect to vehicle #%1, which sends MAVLink version %3 (%2 uses version %4).").arg(vehicleId).arg(qgcApp()->applicationName()).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION)); - // return; - // } - Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager); connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1); connect(vehicle, &Vehicle::requestProtocolVersion, this, &MultiVehicleManager::_requestProtocolVersion); diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index 5b2cfe6cabedad60c9dd76ff890ee806391d21ab..697d30d005e71c0faccf1f43f9fbb4dc61a57672 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -94,7 +94,7 @@ private slots: void _setActiveVehiclePhase2(void); void _vehicleParametersReadyChanged(bool parametersReady); void _sendGCSHeartbeat(void); - void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); + void _vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); void _requestProtocolVersion(unsigned version); private: diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 64e27fa4774ce233995443e142a46f2415fa3102..21fca049a3bf9cdf98b0cc566dbd37c102eeaef9 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -233,18 +233,27 @@ Vehicle::Vehicle(LinkInterface* link, connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64))); connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64))); + if (_highLatencyLink) { + // High latency links don't request information + _setMaxProtoVersion(100); + _setCapabilities(0); + _initialPlanRequestComplete = true; + _missionManagerInitialRequestSent = true; + _geoFenceManagerInitialRequestSent = true; + _rallyPointManagerInitialRequestSent = true; + } else { + // Ask the vehicle for protocol version info. + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_PROTOCOL_VERSION, + false, // No error shown if fails + 1); // Request protocol version - // Ask the vehicle for protocol version info. - sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. - MAV_CMD_REQUEST_PROTOCOL_VERSION, - false, // No error shown if fails - 1); // Request protocol version - - // Ask the vehicle for firmware version info. - sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. - MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, - false, // No error shown if fails - 1); // Request firmware version + // Ask the vehicle for firmware version info. + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + false, // No error shown if fails + 1); // Request firmware version + } _firmwarePlugin->initializeVehicle(this); @@ -677,13 +686,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes break; case MAVLINK_MSG_ID_SCALED_PRESSURE3: _handleScaledPressure3(message); - break; + break; case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: _handleCameraImageCaptured(message); break; case MAVLINK_MSG_ID_ADSB_VEHICLE: _handleADSBVehicle(message); break; + case MAVLINK_MSG_ID_HIGH_LATENCY2: + _handleHighLatency2(message); + break; case MAVLINK_MSG_ID_SERIAL_CONTROL: { @@ -693,7 +705,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } break; - // Following are ArduPilot dialect messages + // Following are ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) case MAVLINK_MSG_ID_CAMERA_FEEDBACK: _handleCameraFeedback(message); @@ -797,6 +809,47 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) emit coordinateChanged(_coordinate); } +void Vehicle::_handleHighLatency2(mavlink_message_t& message) +{ + mavlink_high_latency2_t highLatency2; + mavlink_msg_high_latency2_decode(&message, &highLatency2); + +#if 0 + typedef struct __mavlink_high_latency2_t { + uint16_t wp_num; /*< Current waypoint number*/ + uint16_t failure_flags; /*< Indicates failures as defined in MAV_FAILURE_FLAG ENUM. */ + uint8_t flight_mode; /*< Flight Mode of the vehicle as defined in the FLIGHT_MODE ENUM*/ + uint8_t failsafe; /*< Indicates if a failsafe mode is triggered, defined in MAV_FAILSAFE_FLAG ENUM*/ + }) mavlink_high_latency2_t; +#endif + + // FIXME: flight mode not yet supported + + _coordinate.setLatitude(highLatency2.latitude / (double)1E7); + _coordinate.setLongitude(highLatency2.longitude / (double)1E7); + _coordinate.setAltitude(highLatency2.altitude); + emit coordinateChanged(_coordinate); + + _airSpeedFact.setRawValue((double)highLatency2.airspeed / 5.0); + _groundSpeedFact.setRawValue((double)highLatency2.groundspeed / 5.0); + _climbRateFact.setRawValue((double)highLatency2.climb_rate / 10.0); + + _headingFact.setRawValue((double)highLatency2.heading * 2.0); + + _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0); + _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0); + + _batteryFactGroup.percentRemaining()->setRawValue(highLatency2.battery); + + _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air); + + _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7); + _gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7); + _gpsFactGroup.count()->setRawValue(0); + _gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits::quiet_NaN() : highLatency2.eph / 10.0); + _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits::quiet_NaN() : highLatency2.epv / 10.0); +} + void Vehicle::_handleAltitude(mavlink_message_t& message) { mavlink_altitude_t altitude; @@ -909,14 +962,14 @@ QString Vehicle::vehicleUIDStr() QString uid; uint8_t* pUid = (uint8_t*)(void*)&_uid; uid.sprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X", - pUid[0] & 0xff, - pUid[1] & 0xff, - pUid[2] & 0xff, - pUid[3] & 0xff, - pUid[4] & 0xff, - pUid[5] & 0xff, - pUid[6] & 0xff, - pUid[7] & 0xff); + pUid[0] & 0xff, + pUid[1] & 0xff, + pUid[2] & 0xff, + pUid[3] & 0xff, + pUid[4] & 0xff, + pUid[5] & 0xff, + pUid[6] & 0xff, + pUid[7] & 0xff); return uid; } @@ -926,22 +979,22 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) mavlink_msg_hil_actuator_controls_decode(&message, &hil); emit hilActuatorControlsChanged(hil.time_usec, hil.flags, hil.controls[0], - hil.controls[1], - hil.controls[2], - hil.controls[3], - hil.controls[4], - hil.controls[5], - hil.controls[6], - hil.controls[7], - hil.controls[8], - hil.controls[9], - hil.controls[10], - hil.controls[11], - hil.controls[12], - hil.controls[13], - hil.controls[14], - hil.controls[15], - hil.mode); + hil.controls[1], + hil.controls[2], + hil.controls[3], + hil.controls[4], + hil.controls[5], + hil.controls[6], + hil.controls[7], + hil.controls[8], + hil.controls[9], + hil.controls[10], + hil.controls[11], + hil.controls[12], + hil.controls[13], + hil.controls[14], + hil.controls[15], + hil.mode); } void Vehicle::_handleCommandLong(mavlink_message_t& message) @@ -954,19 +1007,19 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message) mavlink_msg_command_long_decode(&message, &cmd); switch (cmd.command) { - // Other component on the same system - // requests that QGC frees up the serial port of the autopilot - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (cmd.param6 > 0) { - // disconnect the USB link if its a direct connection to a Pixhawk - for (int i = 0; i < _links.length(); i++) { - SerialLink *sl = qobject_cast(_links.at(i)); - if (sl && sl->getSerialConfig()->usbDirect()) { - qDebug() << "Disconnecting:" << _links.at(i)->getName(); - qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); - } + // Other component on the same system + // requests that QGC frees up the serial port of the autopilot + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (cmd.param6 > 0) { + // disconnect the USB link if its a direct connection to a Pixhawk + for (int i = 0; i < _links.length(); i++) { + SerialLink *sl = qobject_cast(_links.at(i)); + if (sl && sl->getSerialConfig()->usbDirect()) { + qDebug() << "Disconnecting:" << _links.at(i)->getName(); + qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); } } + } break; } #endif @@ -1351,6 +1404,7 @@ void Vehicle::_addLink(LinkInterface* link) qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); _links += link; _updatePriorityLink(); + _updateHighLatencyLink(); connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted); connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted); connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink); @@ -2098,9 +2152,9 @@ void Vehicle::_connectionActive(void) // Re-negotiate protocol version for the link sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. - MAV_CMD_REQUEST_PROTOCOL_VERSION, + MAV_CMD_REQUEST_PROTOCOL_VERSION, false, // No error shown if fails - 1); // Request protocol version + 1); // Request protocol version } } @@ -2618,7 +2672,7 @@ void Vehicle::setSoloFirmware(bool soloFirmware) } #if 0 - // Temporarily removed, waiting for new command implementation +// Temporarily removed, waiting for new command implementation void Vehicle::motorTest(int motor, int percent, int timeoutSecs) { doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs); @@ -2765,11 +2819,11 @@ void Vehicle::_ackMavlinkLogData(uint16_t sequence) ack.target_component = _defaultComponentId; ack.target_system = id(); mavlink_msg_logging_ack_encode_chan( - _mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - &ack); + _mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &ack); sendMessageOnLink(priorityLink(), msg); } @@ -2778,7 +2832,7 @@ void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) mavlink_logging_data_t log; mavlink_msg_logging_data_decode(&message, &log); emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, - log.first_message_offset, QByteArray((const char*)log.data, log.length), false); + log.first_message_offset, QByteArray((const char*)log.data, log.length), false); } void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) @@ -2787,7 +2841,7 @@ void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) mavlink_msg_logging_data_acked_decode(&message, &log); _ackMavlinkLogData(log.sequence); emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, - log.first_message_offset, QByteArray((const char*)log.data, log.length), true); + log.first_message_offset, QByteArray((const char*)log.data, log.length), true); } void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData) @@ -2946,7 +3000,7 @@ QString Vehicle::hobbsMeter() static const char* HOOBS_LO = "LND_FLIGHT_T_LO"; //-- TODO: Does this exist on non PX4? if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) && - _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) { + _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) { Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI); Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO); uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 8159e2c4ea99f0ccaeff53b494ce01c2ca3f9ab7..35ba2a188856a859508bc27a21e3f594b9c9a68c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -915,6 +915,7 @@ private: void _handleScaledPressure(mavlink_message_t& message); void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message); + void _handleHighLatency2(mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); diff --git a/src/VehicleSetup/SetupView.qml b/src/VehicleSetup/SetupView.qml index 1fb0418e5932cd0c319861f9b6ecae543f6d623a..9b94e26c72a696d83d3ecf87b62fc3b82a605b73 100644 --- a/src/VehicleSetup/SetupView.qml +++ b/src/VehicleSetup/SetupView.qml @@ -299,7 +299,9 @@ Rectangle { SubMenuButton { setupIndicator: false exclusiveGroup: setupButtonGroup - visible: QGroundControl.multiVehicleManager && QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable && _corePlugin.showAdvancedUI + visible: QGroundControl.multiVehicleManager.parameterReadyVehicleAvailable && + !QGroundControl.multiVehicleManager.activeVehicle.highLatencyLink && + _corePlugin.showAdvancedUI text: qsTr("Parameters") Layout.fillWidth: true diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 9052ad608fcf52869de9edf186231c3692faac7a..d517a9a67b50b28e22c67f4a5dc94af341edf060 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -267,11 +267,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) } if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { - // Start loggin on first heartbeat _startLogging(); mavlink_heartbeat_t heartbeat; mavlink_msg_heartbeat_decode(&message, &heartbeat); - emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type); + emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.autopilot, heartbeat.type); + } + + if (message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) { + _startLogging(); + mavlink_high_latency2_t highLatency2; + mavlink_msg_high_latency2_decode(&message, &highLatency2); + emit vehicleHeartbeatInfo(link, message.sysid, message.compid, highLatency2.autopilot, highLatency2.type); } // Detect if we are talking to an old radio not supporting v2 diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 5d6311734020c37b270cc08cb8656cb58c0f337a..4d6ae9f79904e681c6f342a50a05c12d91b3cb0b 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -137,7 +137,7 @@ protected: signals: /// Heartbeat received on link - void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType); + void vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); /** @brief Message received and directly copied via signal */ void messageReceived(LinkInterface* link, mavlink_message_t message); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 013e81e9538baa2baffd8f346005e5b2da0d541e..e2ce6a9959e0f4d56c44e4f87d66fc31333c4b13 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -13,7 +13,7 @@ #include "QGCApplication.h" #ifdef UNITTEST_BUILD - #include "UnitTest.h" +#include "UnitTest.h" #endif #include @@ -166,27 +166,35 @@ void MockLink::run(void) void MockLink::_run1HzTasks(void) { if (_mavlinkStarted && _connected) { - _sendVibration(); - _sendADSBVehicles(); - if (!qgcApp()->runningUnitTests()) { - // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation - _sendRCChannels(); - } - if (_sendHomePositionDelayCount > 0) { - // We delay home position for better testing - _sendHomePositionDelayCount--; + if (_highLatency) { + _sendHighLatency2(); } else { - _sendHomePosition(); - } - if (_sendStatusText) { - _sendStatusText = false; - _sendStatusTextMessages(); + _sendVibration(); + _sendADSBVehicles(); + if (!qgcApp()->runningUnitTests()) { + // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation + _sendRCChannels(); + } + if (_sendHomePositionDelayCount > 0) { + // We delay home position for better testing + _sendHomePositionDelayCount--; + } else { + _sendHomePosition(); + } + if (_sendStatusText) { + _sendStatusText = false; + _sendStatusTextMessages(); + } } } } void MockLink::_run10HzTasks(void) { + if (_highLatency) { + return; + } + if (_mavlinkStarted && _connected) { _sendHeartBeat(); if (_sendGPSPositionDelayCount > 0) { @@ -200,6 +208,10 @@ void MockLink::_run10HzTasks(void) void MockLink::_run500HzTasks(void) { + if (_highLatency) { + return; + } + if (_mavlinkStarted && _connected) { _paramRequestListWorker(); _logDownloadWorker(); @@ -296,6 +308,44 @@ void MockLink::_sendHeartBeat(void) respondWithMavlinkMessage(msg); } +void MockLink::_sendHighLatency2(void) +{ + mavlink_message_t msg; + + mavlink_msg_high_latency2_pack_chan(_vehicleSystemId, + _vehicleComponentId, + _mavlinkChannel, + &msg, + 0, // timestamp + _vehicleType, // MAV_TYPE + _firmwareType, // MAV_AUTOPILOT + _flightModeEnumValue(), // flight_mode + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int16_t)_vehicleAltitude, + (int16_t)_vehicleAltitude, // target_altitude, + 0, // heading + 0, // target_heading + 0, // target_distance + 0, // throttle + 0, // airspeed + 0, // airspeed_sp + 0, // groundspeed + 0, // windspeed, + 0, // wind_heading + UINT8_MAX, // eph not known + UINT8_MAX, // epv not known + 0, // temperature_air + 0, // climb_rate + -1, // battery, do not use? + 0, // wp_num + 0, // failure_flags + 0, // failsafe + 0, 0, 0); // custom0, custom1, custom2 + + respondWithMavlinkMessage(msg); +} + void MockLink::_sendVibration(void) { mavlink_message_t msg; @@ -842,8 +892,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) case MAV_CMD_USER_3: // Test command which returns MAV_RESULT_ACCEPTED on second attempt if (firstCmdUser3) { - firstCmdUser3 = false; - return; + firstCmdUser3 = false; + return; } else { firstCmdUser3 = true; commandResult = MAV_RESULT_ACCEPTED; @@ -852,8 +902,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) case MAV_CMD_USER_4: // Test command which returns MAV_RESULT_FAILED on second attempt if (firstCmdUser4) { - firstCmdUser4 = false; - return; + firstCmdUser4 = false; + return; } else { firstCmdUser4 = true; commandResult = MAV_RESULT_FAILED; @@ -943,8 +993,8 @@ void MockLink::_sendHomePosition(void) (int32_t)(_vehicleAltitude * 1000), 0.0f, 0.0f, 0.0f, &bogus[0], - 0.0f, 0.0f, 0.0f, - 0); + 0.0f, 0.0f, 0.0f, + 0); respondWithMavlinkMessage(msg); } @@ -972,7 +1022,7 @@ void MockLink::_sendGpsRawInt(void) 0, // Altitude uncertainty in meters * 1000 (positive for up). 0, // Speed uncertainty in meters * 1000 (positive for up). 0); // Heading / track uncertainty in degrees * 1e5. - respondWithMavlinkMessage(msg); + respondWithMavlinkMessage(msg); } void MockLink::_sendStatusTextMessages(void) @@ -1237,9 +1287,9 @@ void MockLink::_handleLogRequestData(const mavlink_message_t& msg) mavlink_msg_log_request_data_decode(&msg, &request); if (_logDownloadFilename.isEmpty()) { - #ifdef UNITTEST_BUILD +#ifdef UNITTEST_BUILD _logDownloadFilename = UnitTest::createRandomFile(_logDownloadFileSize); - #endif +#endif } if (request.id != 0) { @@ -1320,3 +1370,8 @@ void MockLink::_sendADSBVehicles(void) respondWithMavlinkMessage(responseMsg); } + +uint8_t MockLink::_flightModeEnumValue(void) +{ + return FLIGHT_MODE_STABILIZED; +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 1eb0b48c155e19198ce0f964aa24a92dae8eadc6..f58730d1f065fa6b8caddd38ed5a96f4ce3637f0 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -175,6 +175,7 @@ private: // MockLink methods void _sendHeartBeat(void); + void _sendHighLatency2(void); void _handleIncomingNSHBytes(const char* bytes, int cBytes); void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes); void _loadParams(void); @@ -201,6 +202,7 @@ private: void _logDownloadWorker(void); void _sendADSBVehicles(void); void _moveADSBVehicle(void); + uint8_t _flightModeEnumValue(void); static MockLink* _startMockLink(MockConfiguration* mockConfig);