diff --git a/src/AnalyzeView/LogDownloadController.cc b/src/AnalyzeView/LogDownloadController.cc index fee51217fe88c7fbd628c78da8d9dfcd7a29ed03..a1b3924c9c297f5c9af9ee71dcb437c4f1716308 100644 --- a/src/AnalyzeView/LogDownloadController.cc +++ b/src/AnalyzeView/LogDownloadController.cc @@ -478,7 +478,7 @@ LogDownloadController::_requestLogData(uint16_t id, uint32_t offset, uint32_t co &msg, qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId(), id, offset, count); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } } @@ -508,7 +508,7 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end) _vehicle->defaultComponentId(), start, end); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); //-- Wait 5 seconds before bitching about not getting anything _timer.start(5000); } @@ -674,7 +674,7 @@ LogDownloadController::eraseAll(void) _vehicle->priorityLink()->mavlinkChannel(), &msg, qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId()); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); refresh(); } } diff --git a/src/AnalyzeView/MavlinkConsoleController.cc b/src/AnalyzeView/MavlinkConsoleController.cc index c08b93f9c1e4170cdfa7d9819456951cea6e34ce..02809e6877c3b118eaa9455aa176c6a8a341c307 100644 --- a/src/AnalyzeView/MavlinkConsoleController.cc +++ b/src/AnalyzeView/MavlinkConsoleController.cc @@ -129,7 +129,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close) 0, chunk.size(), reinterpret_cast(chunk.data())); - _vehicle->sendMessageOnLink(priority_link, msg); + _vehicle->sendMessageOnLinkThreadSafe(priority_link, msg); data.remove(0, chunk.size()); } } diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 2bd404b5d581e7ccda95d58dbbbf883bb462e1d0..758d30039e22a4c973722cdbc8f4242e6841d0c4 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -704,7 +704,7 @@ void APMSensorsComponentController::nextClicked(void) 0, // target_system 0); // target_component - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); if (_calTypeInProgress == CalTypeCompassMot) { _stopCalibration(StopCalibrationSuccess); diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 3a5d8a5cf2ada70426b017ef022b0ed75cfdf1e2..2c822b7f35321e020e4ecdfbbecf2b84713bf89a 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -1183,7 +1183,7 @@ QGCCameraControl::_requestAllParameters() &msg, static_cast(_vehicle->id()), static_cast(compID())); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); qCDebug(CameraControlVerboseLog) << "Request all parameters"; } diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index 3ef7450d452f540fbc29a332fb7af898acb923a1..a0cbb4cc36576589ec402fd0e4438ebdc93d7efd 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -191,7 +191,7 @@ QGCCameraParamIO::_sendParameter() _vehicle->priorityLink()->mavlinkChannel(), &msg, &p); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); _paramWriteTimer.start(); } @@ -364,6 +364,6 @@ QGCCameraParamIO::paramRequest(bool reset) static_cast(_control->compID()), param_id, -1); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); _paramRequestTimer.start(); } diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index 820516db1c2015f691d7b46188d21e3a396c0212..9b4575908b8a1005c5498d507444853ee76c4467 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -526,7 +526,7 @@ void ParameterManager::refreshAllParameters(uint8_t componentId) &msg, _vehicle->id(), componentId); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); QString what = (componentId == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentId); qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Request to refresh all parameters for component ID:" << what; @@ -831,7 +831,7 @@ void ParameterManager::_readParameterRaw(int componentId, const QString& paramNa componentId, // Target component id fixedParamName, // Named parameter being requested paramIndex); // Parameter index being requested, -1 for named - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } void ParameterManager::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value) @@ -890,7 +890,7 @@ void ParameterManager::_writeParameterRaw(int componentId, const QString& paramN _vehicle->priorityLink()->mavlinkChannel(), &msg, &p); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } void ParameterManager::_writeLocalParamCache(int vehicleId, int componentId) @@ -997,7 +997,7 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian _vehicle->priorityLink()->mavlinkChannel(), &msg, &p); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); // Give the user some feedback things loaded properly QVariantAnimation *ani = new QVariantAnimation(this); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index f5e303fc929152fd45c58004a1b327df42227279..746bbe0734308399c84c5ac4b9b1ad60505aa901 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -945,7 +945,7 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu &msg, &cmd); - vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); + vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), msg); } void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) @@ -1139,5 +1139,5 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti vehicle->priorityLink()->mavlinkChannel(), &message, &globalPositionInt); - vehicle->sendMessageOnLink(vehicle->priorityLink(), message); + vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 7b862974e40170552788760a20da27408b1844d1..99b47d6081aa6bcfc3ae5ee47361e164bb22cea7 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -947,5 +947,5 @@ void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionRe vehicle->priorityLink()->mavlinkChannel(), &message, &follow_target); - vehicle->sendMessageOnLink(vehicle->priorityLink(), message); + vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message); } diff --git a/src/FlightDisplay/VirtualJoystick.qml b/src/FlightDisplay/VirtualJoystick.qml index 5be34717a35b92bdef19f68c4302c321ad0fcdd8..8ae010b41d3ed36fc5530cd30323116d4aae5bd3 100644 --- a/src/FlightDisplay/VirtualJoystick.qml +++ b/src/FlightDisplay/VirtualJoystick.qml @@ -28,7 +28,7 @@ Item { repeat: true onTriggered: { if (activeVehicle) { - activeVehicle.virtualTabletJoystickValue(rightStick.xAxis, -rightStick.yAxis, leftStick.xAxis, leftStick.yAxis) + activeVehicle.virtualTabletJoystickValue(rightStick.xAxis, rightStick.yAxis, leftStick.xAxis, leftStick.yAxis) } } } diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index e45c84ef71979a7f040927eb5d492bf901fccee1..dd9557cf67d185b257c20ee20ad853cdb9adb686 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -72,6 +72,6 @@ void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) vehicle->priorityLink()->mavlinkChannel(), &message, &msg); - vehicle->sendMessageOnLink(vehicle->priorityLink(), message); + vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message); } } diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index cd7aab9e103102809e207f170af1d336ae2f8a3f..ceb016638143a0566fe7e9088ef47aa9a36260a7 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -117,8 +117,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC Joystick::~Joystick() { - // Crash out of the thread if it is still running - terminate(); + _exitThread = true; wait(); delete[] _rgAxisValues; delete[] _rgCalibration; @@ -636,9 +635,9 @@ void Joystick::_handleAxis() // Exponential (0% to -50% range like most RC radios) // _exponential is set by a slider in joystickConfigAdvanced.qml // Calculate new RPY with exponential applied - roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; - pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; - yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; + roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; + pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; + yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; } // Adjust throttle to 0:1 range @@ -661,7 +660,8 @@ void Joystick::_handleAxis() } } uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); - emit manualControl(roll, -pitch, yaw, throttle, shortButtons, _activeVehicle->joystickMode()); + _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons); + emit axisValues(roll, -pitch, yaw, throttle); // Used by joystick cal screen if(_activeVehicle && _axisCount > 4 && _gimbalEnabled) { //-- TODO: There is nothing consuming this as there are no messages to handle gimbal // the way MANUAL_CONTROL handles the other channels. @@ -676,8 +676,6 @@ void Joystick::startPolling(Vehicle* vehicle) if (vehicle) { // If a vehicle is connected, disconnect it if (_activeVehicle) { - UAS* uas = _activeVehicle->uas(); - disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); @@ -700,8 +698,6 @@ void Joystick::startPolling(Vehicle* vehicle) // Only connect the new vehicle if it wants joystick data if (vehicle->joystickEnabled()) { _pollingStartedForCalibration = false; - UAS* uas = _activeVehicle->uas(); - connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); @@ -710,8 +706,6 @@ void Joystick::startPolling(Vehicle* vehicle) connect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); - // FIXME: **** - //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); } } if (!isRunning()) { @@ -724,10 +718,6 @@ void Joystick::stopPolling(void) { if (isRunning()) { if (_activeVehicle && _activeVehicle->joystickEnabled()) { - UAS* uas = _activeVehicle->uas(); - // Neutral attitude controls - // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode()); - disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); @@ -736,8 +726,6 @@ void Joystick::stopPolling(void) disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); } - // FIXME: **** - //disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); _exitThread = true; } } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 571d9cc41659e7345ede2e30d9fc343983940031..c02f475915b46dfef74027073834a9e8de0c1bb5 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -193,19 +193,9 @@ signals: void accumulatorChanged (bool accumulator); void enabledChanged (bool enabled); void circleCorrectionChanged (bool circleCorrection); - - /// Signal containing new joystick information - /// @param roll: Range is -1:1, negative meaning roll left, positive meaning roll right - /// @param pitch: Range i -1:1, negative meaning pitch down, positive meaning pitch up - /// @param yaw: Range is -1:1, negative meaning yaw left, positive meaning yaw right - /// @param throttle: Range is 0:1, 0 meaning no throttle, 1 meaning full throttle - /// @param buttons: Button bitmap - /// @param joystickMmode: Current joystick mode - void manualControl (float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode); + void axisValues (float roll, float pitch, float yaw, float throttle); void manualControlGimbal (float gimbalPitch, float gimbalYaw); - void buttonActionTriggered (int action); - void gimbalEnabledChanged (); void axisFrequencyChanged (); void buttonFrequencyChanged (); diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 08d1d54a393fef61047e8567b854a585ae0fc6c6..348777ae7f0f57acdc2428f8d25fcd7a41402f1d 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -69,7 +69,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC &messageOut, &missionItem); - _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, messageOut); _startAckTimeout(AckGuidedItem); emit inProgressChanged(true); } diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 6a08a88228f51b4e5cf13d486d7fd4e9c5d798b1..98223a6d3269e4fa562bdf1b58e227adc03e2e88 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -119,7 +119,7 @@ void PlanManager::_writeMissionCount(void) _writeMissionItems.count(), _planType); - _vehicle->sendMessageOnLink(_dedicatedLink, message); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message); _startAckTimeout(AckMissionRequest); } @@ -161,7 +161,7 @@ void PlanManager::_requestList(void) MAV_COMP_ID_AUTOPILOT1, _planType); - _vehicle->sendMessageOnLink(_dedicatedLink, message); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message); _startAckTimeout(AckMissionCount); } @@ -301,7 +301,7 @@ void PlanManager::_readTransactionComplete(void) MAV_MISSION_ACCEPTED, _planType); - _vehicle->sendMessageOnLink(_dedicatedLink, message); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message); _finishTransaction(true); } @@ -369,7 +369,7 @@ void PlanManager::_requestNextMissionItem(void) _planType); } - _vehicle->sendMessageOnLink(_dedicatedLink, message); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, message); _startAckTimeout(AckMissionItem); } @@ -586,7 +586,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m _planType); } - _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); + _vehicle->sendMessageOnLinkThreadSafe(_dedicatedLink, messageOut); _startAckTimeout(AckMissionRequest); } @@ -928,7 +928,7 @@ void PlanManager::_removeAllWorker(void) _vehicle->id(), MAV_COMP_ID_AUTOPILOT1, _planType); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), message); _startAckTimeout(AckMissionClearAll); } diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 1da901122886319014d4521cdb13a344e3e9be2c..db36fe38f1cc6e35213a8786885156b1e6919dae 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -387,8 +387,7 @@ void MultiVehicleManager::_sendGCSHeartbeat(void) uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &message); - - link->writeBytesSafe((const char*)buffer, len); + link->writeBytesThreadSafe((const char*)buffer, len); } } } diff --git a/src/Vehicle/TerrainProtocolHandler.cc b/src/Vehicle/TerrainProtocolHandler.cc index 76b10a614f64a2523981fe50a88318bf59897f5e..e4ed7fcb5d343cd22fe84ebcd69831a169f40ae7 100644 --- a/src/Vehicle/TerrainProtocolHandler.cc +++ b/src/Vehicle/TerrainProtocolHandler.cc @@ -146,7 +146,7 @@ void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, ui _currentTerrainRequest.grid_spacing, gridBit, terrainData); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 3dd30e93ca5ec903b1e90147db8210a622ce35aa..cf9b8d79a46d2b5cce261d192bcfd87f2a25bbff 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -59,7 +59,6 @@ QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle."); const char* Vehicle::_settingsGroup = "Vehicle%1"; // %1 replaced with mavlink system id -const char* Vehicle::_joystickModeSettingsKey = "JoystickMode"; const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; const char* Vehicle::_rollFactName = "roll"; @@ -116,7 +115,6 @@ Vehicle::Vehicle(LinkInterface* link, , _soloFirmware(false) , _toolbox(qgcApp()->toolbox()) , _settingsManager(_toolbox->settingsManager()) - , _joystickMode(JoystickModeRC) , _joystickEnabled(false) , _uas(nullptr) , _mav(nullptr) @@ -235,7 +233,6 @@ Vehicle::Vehicle(LinkInterface* link, _addLink(link); - connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged); connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged); @@ -328,7 +325,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _soloFirmware(false) , _toolbox(qgcApp()->toolbox()) , _settingsManager(_toolbox->settingsManager()) - , _joystickMode(JoystickModeRC) , _joystickEnabled(false) , _uas(nullptr) , _mav(nullptr) @@ -1846,7 +1842,7 @@ void Vehicle::_handlePing(LinkInterface* link, mavlink_message_t& message) ping.seq, message.sysid, message.compid); - sendMessageOnLink(link, msg); + sendMessageOnLinkThreadSafe(link, msg); } void Vehicle::_handleHeartbeat(mavlink_message_t& message) @@ -2104,30 +2100,14 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) } } -bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message) +bool Vehicle::sendMessageOnLinkThreadSafe(LinkInterface* link, mavlink_message_t message) { - if (!link || !_links.contains(link) || !link->isConnected()) { - return false; - } - - emit _sendMessageOnLinkOnThread(link, message); - - return true; -} + QMutexLocker lock(&_sendMessageOnLinkMutex); -void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) -{ - // Make sure this is still a good link - if (!link || !_links.contains(link) || !link->isConnected()) { - return; + if (!link->isConnected()) { + return false; } -#if 0 - // Leaving in for ease in Mav 2.0 testing - mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel()); - qDebug() << "_sendMessageOnLink" << mavlinkStatus << link->mavlinkChannel() << mavlinkStatus->flags << message.magic; -#endif - // Give the plugin a chance to adjust _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message); @@ -2135,9 +2115,11 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &message); - link->writeBytesSafe((const char*)buffer, len); + link->writeBytesThreadSafe((const char*)buffer, len); _messagesSent++; emit messagesSentChanged(); + + return true; } void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand) @@ -2426,11 +2408,6 @@ void Vehicle::_loadSettings() } QSettings settings; settings.beginGroup(QString(_settingsGroup).arg(_id)); - bool convertOk; - _joystickMode = static_cast(settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk)); - if (!convertOk) { - _joystickMode = JoystickModeRC; - } // Joystick enabled is a global setting so first make sure there are any joysticks connected if (_toolbox->joystickManager()->joysticks().count()) { setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool()); @@ -2441,11 +2418,8 @@ void Vehicle::_loadSettings() void Vehicle::_saveSettings() { QSettings settings; - settings.beginGroup(QString(_settingsGroup).arg(_id)); - settings.setValue(_joystickModeSettingsKey, _joystickMode); - // The joystick enabled setting should only be changed if a joystick is present // since the checkbox can only be clicked if one is present if (_toolbox->joystickManager()->joysticks().count()) { @@ -2453,32 +2427,6 @@ void Vehicle::_saveSettings() } } -int Vehicle::joystickMode() -{ - return _joystickMode; -} - -void Vehicle::setJoystickMode(int mode) -{ - if (mode < 0 || mode >= JoystickModeMax) { - qCWarning(VehicleLog) << "Invalid joystick mode" << mode; - return; - } - - _joystickMode = (JoystickMode_t)mode; - _saveSettings(); - emit joystickModeChanged(mode); -} - -QStringList Vehicle::joystickModes() -{ - QStringList list; - - list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity"; - - return list; -} - bool Vehicle::joystickEnabled() { return _joystickEnabled; @@ -2571,7 +2519,7 @@ void Vehicle::setFlightMode(const QString& flightMode) id(), newBaseMode, custom_mode); - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } else { qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; } @@ -2647,7 +2595,7 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool send // We use sendMessageMultiple since we really want these to make it to the vehicle sendMessageMultiple(msg); } else { - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } } @@ -2656,7 +2604,7 @@ void Vehicle::_sendMessageMultipleNext() if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) { qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; - sendMessageOnLink(priorityLink(), _sendMessageMultipleList[_nextSendMessageMultipleIndex].message); + sendMessageOnLinkThreadSafe(priorityLink(), _sendMessageMultipleList[_nextSendMessageMultipleIndex].message); if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) { _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex); @@ -2826,7 +2774,7 @@ void Vehicle::_sendQGCTimeToVehicle() &msg, &cmd); - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } void Vehicle::disconnectInactiveVehicle() @@ -2884,13 +2832,13 @@ void Vehicle::_remoteControlRSSIChanged(uint8_t rssi) void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust) { // The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled - if ( !_joystickEnabled && !_highLatencyLink) { - _uas->setExternalControlSetpoint( + if (!_joystickEnabled) { + sendJoystickDataThreadSafe( static_cast(roll), static_cast(pitch), static_cast(yaw), static_cast(thrust), - 0, JoystickModeRC); + 0); } } @@ -3362,7 +3310,7 @@ void Vehicle::setCurrentMissionSequence(int seq) static_cast(id()), _compID, static_cast(seq)); - sendMessageOnLink(priorityLink(), msg); + 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) @@ -3500,7 +3448,7 @@ void Vehicle::_sendMavCommandAgain() &cmd); } - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } void Vehicle::_sendNextQueuedMavCommand() @@ -3691,7 +3639,7 @@ void Vehicle::rebootVehicle() priorityLink()->mavlinkChannel(), &msg, &cmd); - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } void Vehicle::setSoloFirmware(bool soloFirmware) @@ -3857,7 +3805,7 @@ void Vehicle::_ackMavlinkLogData(uint16_t sequence) priorityLink()->mavlinkChannel(), &msg, &ack); - sendMessageOnLink(priorityLink(), msg); + sendMessageOnLinkThreadSafe(priorityLink(), msg); } void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) @@ -4426,7 +4374,7 @@ void Vehicle::sendParamMapRC(const QString& paramName, double scale, double cent static_cast(centerValue), static_cast(minValue), static_cast(maxValue)); - sendMessageOnLink(priorityLink(), message); + sendMessageOnLinkThreadSafe(priorityLink(), message); } void Vehicle::clearAllParamMapRC(void) @@ -4445,10 +4393,39 @@ void Vehicle::clearAllParamMapRC(void) -2, // Disable map for specified tuning id i, // tuning id 0, 0, 0, 0); // unused - sendMessageOnLink(priorityLink(), message); + sendMessageOnLinkThreadSafe(priorityLink(), message); } } +void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, float thrust, quint16 buttons) +{ + if (_highLatencyLink) { + return; + } + + mavlink_message_t message; + + // Incoming values are in the range -1:1 + float axesScaling = 1.0 * 1000.0; + float newRollCommand = roll * axesScaling; + float newPitchCommand = pitch * axesScaling; // Joystick data is reverse of mavlink values + float newYawCommand = yaw * axesScaling; + float newThrustCommand = thrust * axesScaling; + + mavlink_msg_manual_control_pack_chan( + static_cast(_mavlink->getSystemId()), + static_cast(_mavlink->getComponentId()), + priorityLink()->mavlinkChannel(), + &message, + static_cast(_id), + static_cast(newPitchCommand), + static_cast(newRollCommand), + static_cast(newThrustCommand), + static_cast(newYawCommand), + buttons); + sendMessageOnLinkThreadSafe(priorityLink(), message); +} + //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2d16619ad453aa4b86ee25d18a74b83d0dd6b2b5..acb89feb35701177dc4948c79a2cba4e1bd16f36 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -561,8 +561,6 @@ public: Q_PROPERTY(QString formatedMessages READ formatedMessages NOTIFY formatedMessagesChanged) Q_PROPERTY(QString formatedMessage READ formatedMessage NOTIFY formatedMessageChanged) Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) - Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged) - Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) @@ -809,25 +807,11 @@ public: QGeoCoordinate coordinate() { return _coordinate; } QGeoCoordinate armedPosition () { return _armedPosition; } - typedef enum { - JoystickModeRC, ///< Joystick emulates an RC Transmitter - JoystickModeAttitude, - JoystickModePosition, - JoystickModeForce, - JoystickModeVelocity, - JoystickModeMax - } JoystickMode_t; - void updateFlightDistance(double distance); - int joystickMode(); - void setJoystickMode(int mode); - - /// List of joystick mode names - QStringList joystickModes(); - - bool joystickEnabled(); - void setJoystickEnabled(bool enabled); + bool joystickEnabled (); + void setJoystickEnabled (bool enabled); + void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons); // Is vehicle active with respect to current active vehicle in QGC bool active(); @@ -845,7 +829,7 @@ public: /// Sends a message to the specified link /// @return true: message sent, false: Link no longer connected - bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message); + bool sendMessageOnLinkThreadSafe(LinkInterface* link, mavlink_message_t message); /// Sends the specified messages multiple times to the vehicle in order to attempt to /// guarantee that it makes it to the vehicle. @@ -1145,7 +1129,6 @@ public slots: signals: void allLinksInactive (Vehicle* vehicle); void coordinateChanged (QGeoCoordinate coordinate); - void joystickModeChanged (int mode); void joystickEnabledChanged (bool enabled); void activeChanged (bool active); void mavlinkMessageReceived (const mavlink_message_t& message); @@ -1180,14 +1163,9 @@ signals: void linksPropertiesChanged (); void textMessageReceived (int uasid, int componentid, int severity, QString text); void checkListStateChanged (); - void messagesReceivedChanged (); void messagesSentChanged (); void messagesLostChanged (); - - /// Used internally to move sendMessage call to main thread - void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message); - void messageTypeChanged (); void newMessageCountChanged (); void messageCountChanged (); @@ -1258,7 +1236,6 @@ signals: private slots: void _mavlinkMessageReceived (LinkInterface* link, mavlink_message_t message); void _linkInactiveOrDeleted (LinkInterface* link); - void _sendMessageOnLink (LinkInterface* link, mavlink_message_t message); void _sendMessageMultipleNext (); void _parametersReady (bool parametersReady); void _remoteControlRSSIChanged (uint8_t rssi); @@ -1388,7 +1365,6 @@ private: QList _links; - JoystickMode_t _joystickMode; bool _joystickEnabled; UAS* _uas; @@ -1436,6 +1412,7 @@ private: bool _highLatencyLink; bool _receivingAttitudeQuaternion; CheckList _checkListState = CheckListNotSetup; + QMutex _sendMessageOnLinkMutex; QGCCameraManager* _cameras; @@ -1648,7 +1625,5 @@ private: // Settings keys static const char* _settingsGroup; - static const char* _joystickModeSettingsKey; static const char* _joystickEnabledSettingsKey; - }; diff --git a/src/VehicleSetup/JoystickConfigAdvanced.qml b/src/VehicleSetup/JoystickConfigAdvanced.qml index 2eb8e9f4e933ba708ecf3238ee558596ef08823a..68930b07f8aa5d24d365a75536343ed3c53206fb 100644 --- a/src/VehicleSetup/JoystickConfigAdvanced.qml +++ b/src/VehicleSetup/JoystickConfigAdvanced.qml @@ -128,22 +128,6 @@ Item { } } //----------------------------------------------------------------- - //-- Mode - QGCLabel { - Layout.alignment: Qt.AlignVCenter - text: qsTr("Joystick mode:") - visible: advancedSettings.checked - } - QGCComboBox { - enabled: advancedSettings.checked - currentIndex: activeVehicle.joystickMode - width: ScreenTools.defaultFontPixelWidth * 20 - model: activeVehicle.joystickModes - onActivated: activeVehicle.joystickMode = index - Layout.alignment: Qt.AlignVCenter - visible: advancedSettings.checked - } - //----------------------------------------------------------------- //-- Axis Message Frequency QGCLabel { text: qsTr("Axis frequency (Hz):") diff --git a/src/VehicleSetup/JoystickConfigGeneral.qml b/src/VehicleSetup/JoystickConfigGeneral.qml index cc84541dea3fd54fdc402216deb5dc95c8231ff1..eb60e785cdac571fffeefa6ae35a78d8d7eede8a 100644 --- a/src/VehicleSetup/JoystickConfigGeneral.qml +++ b/src/VehicleSetup/JoystickConfigGeneral.qml @@ -200,7 +200,7 @@ Item { Connections { target: _activeJoystick - onManualControl: { + onAxisValues: { rollAxis.axisValue = roll * 32768.0 pitchAxis.axisValue = pitch * 32768.0 yawAxis.axisValue = yaw * 32768.0 diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index fe1ede86cddf3866f472b90fefa1f9f8596e23ac..1aea6404c8f38c97d1a6e4186722407245c989bb 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -65,7 +65,6 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config, bool isPX4F memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts)); memset(_outDataWriteTimes, 0, sizeof(_outDataWriteTimes)); - QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes); qRegisterMetaType("LinkInterface*"); } @@ -210,3 +209,11 @@ void LinkInterface::stopMavlinkMessagesTimer() { _mavlinkMessagesTimers.clear(); } + +void LinkInterface::writeBytesThreadSafe(const char *bytes, int length) +{ + QByteArray byteArray(bytes, length); + _writeBytesMutex.lock(); + _writeBytes(byteArray); + _writeBytesMutex.unlock(); +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index dbe5d43c8a5293f38f099065e14f161453b65239..ed639d360f1d8d8d4c960c64b07fb8e6e114dd78 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -136,33 +136,11 @@ public: bool connect(void); bool disconnect(void); -public slots: + void writeBytesThreadSafe(const char *bytes, int length); - /** - * @brief This method allows to write bytes to the interface. - * - * If the underlying communication is packet oriented, - * one write command equals a datagram. In case of serial - * communication arbitrary byte lengths can be written. The method ensures - * thread safety regardless of the underlying LinkInterface implementation. - * - * @param bytes: The pointer to the byte array containing the data - * @param length: The length of the data array - **/ - void writeBytesSafe(const char *bytes, int length) - { - emit _invokeWriteBytes(QByteArray(bytes, length)); - } - -private slots: - virtual void _writeBytes(const QByteArray) = 0; - - void _activeChanged(bool active, int vehicle_id); - signals: void autoconnectChanged(bool autoconnect); void activeChanged(LinkInterface* link, bool active, int vehicle_id); - void _invokeWriteBytes(QByteArray); void highLatencyChanged(bool highLatency); /// Signalled when a link suddenly goes away due to it being removed by for example pulling the cable to the connection. @@ -231,6 +209,9 @@ protected: SharedLinkConfigurationPointer _config; bool _highLatency; +private slots: + void _activeChanged(bool active, int vehicle_id); + private: /** * @brief logDataRateToBuffer Stores transmission times/amounts for statistics @@ -288,6 +269,8 @@ private: */ void stopMavlinkMessagesTimer(); + virtual void _writeBytes(const QByteArray) = 0; // Not thread safe, only writeBytesThreadSafe is thread safe + bool _mavlinkChannelSet; ///< true: _mavlinkChannel has been set uint8_t _mavlinkChannel; ///< mavlink channel to use for this link, as used by mavlink_parse_char @@ -307,7 +290,8 @@ private: quint64 _outDataWriteAmounts[_dataRateBufferSize]; // In bytes qint64 _outDataWriteTimes[_dataRateBufferSize]; // in ms - mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables + mutable QMutex _dataRateMutex; + mutable QMutex _writeBytesMutex; bool _enableRateCollection; bool _decodedFirstMavlinkPacket; ///< true: link has correctly decoded it's first mavlink packet diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 86bf1f322573c29da17e140db98736d7acc968a9..51ccdee79742e1e653225361d6a7170d390b2ee8 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -969,7 +969,7 @@ void LinkManager::_activeLinkCheck(void) if (!found && link) { // See if we can get an NSH prompt on this link bool foundNSHPrompt = false; - link->writeBytesSafe("\r", 1); + link->writeBytesThreadSafe("\r", 1); QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray))); if (spy.wait(100)) { QList arguments = spy.takeFirst(); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 7068f75bb179b62976d7f86c0eab181bc646aa73..b32119d96cd4287c558c953b062a8cfef1dcd3f1 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -277,7 +277,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (forwardingLink) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buf, &_message); - forwardingLink->writeBytesSafe((const char*)buf, len); + forwardingLink->writeBytesThreadSafe((const char*)buf, len); } } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index d7c0d4fc9cfd6a2515a50e649aef335230aed0e7..0c560062ff990efce0de84ab1d98e4aa820ae930 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -89,8 +89,9 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) _highLatency = mockConfig->highLatency(); _failureMode = mockConfig->failureMode(); - union px4_custom_mode px4_cm; + QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); + union px4_custom_mode px4_cm; px4_cm.data = 0; px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; _mavCustomMode = px4_cm.data; @@ -411,6 +412,12 @@ void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg) /// @brief Called when QGC wants to write bytes to the MAV void MockLink::_writeBytes(const QByteArray bytes) +{ + // This prevents the responses to mavlink messages from being sent until the _writeBytes returns. + emit writeBytesQueuedSignal(bytes); +} + +void MockLink::_writeBytesQueued(const QByteArray bytes) { if (_inNSH) { _handleIncomingNSHBytes(bytes.constData(), bytes.count()); @@ -422,6 +429,7 @@ void MockLink::_writeBytes(const QByteArray bytes) _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count()); } + } /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 34ae7db43a2eb88c796f60ab0bf9873af757e250..8de34b67cef47b9e50f32963e6e2565c60f9562b 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -160,8 +160,12 @@ public: static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); +signals: + void writeBytesQueuedSignal(const QByteArray bytes); + private slots: - virtual void _writeBytes(const QByteArray bytes); + void _writeBytes(const QByteArray bytes) final; + void _writeBytesQueued(const QByteArray bytes); private slots: void _run1HzTasks(void); diff --git a/src/qgcunittest/TCPLinkTest.cc b/src/qgcunittest/TCPLinkTest.cc index 012caf98ea4932da5fa29d5d29b7680c6b729e44..c42948e45ae41c027c98bcc0c3ff7b7d2834df93 100644 --- a/src/qgcunittest/TCPLinkTest.cc +++ b/src/qgcunittest/TCPLinkTest.cc @@ -125,7 +125,7 @@ void TCPLinkTest::_connectSucceed_test(void) const char* bytesWrittenSignal = SIGNAL(bytesWritten(qint64)); MultiSignalSpy bytesWrittenSpy; QCOMPARE(bytesWrittenSpy.init(_link->getSocket(), &bytesWrittenSignal, 1), true); - _link->writeBytesSafe(bytesOut.data(), bytesOut.size()); + _link->writeBytesThreadSafe(bytesOut.data(), bytesOut.size()); _multiSpy->clearAllSignals(); // We emit this signal such that it will be queued and run on the TCPLink thread. This in turn diff --git a/src/uas/FileManager.cc b/src/uas/FileManager.cc index f985dae850ccc9a3ed44e4643d44632988398995..84b94a8ed3e03a5da60738507ac749cfc9780326 100644 --- a/src/uas/FileManager.cc +++ b/src/uas/FileManager.cc @@ -891,5 +891,5 @@ void FileManager::_sendRequestNoAck(Request* request) 0, // Target component (uint8_t*)request); // Payload - _vehicle->sendMessageOnLink(link, message); + _vehicle->sendMessageOnLinkThreadSafe(link, message); } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b789b7f4b0eb35782e0c8b9ed09b8511dbf3ed63..b9af1a20f664b06684dfa071935d6d2b35310ecd 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -424,7 +424,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) accelCal, // accel cal airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot escCal); // esc cal - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } void UAS::stopCalibration(void) @@ -732,7 +732,7 @@ void UAS::requestImage() &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); } } @@ -811,196 +811,6 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); } -/** -* Set the manual control commands. -* This can only be done if the system has manual inputs enabled and is armed. -*/ -void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) -{ - if (!_vehicle || !_vehicle->priorityLink()) { - return; - } - mavlink_message_t message; - if (joystickMode == Vehicle::JoystickModeAttitude) { - // send an external attitude setpoint command (rate control disabled) - float attitudeQuaternion[4]; - mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); - uint8_t typeMask = 0x7; // disable rate control - mavlink_msg_set_attitude_target_pack_chan( - mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - typeMask, - attitudeQuaternion, - 0, - 0, - 0, - thrust); - } else if (joystickMode == Vehicle::JoystickModePosition) { - // Send the the local position setpoint (local pos sp external message) - static float px = 0; - static float py = 0; - static float pz = 0; - //XXX: find decent scaling - px -= pitch; - py += roll; - pz -= 2.0f*(thrust-0.5); - uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control - mavlink_msg_set_position_target_local_ned_pack_chan( - mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - px, - py, - pz, - 0, - 0, - 0, - 0, - 0, - 0, - yaw, - 0); - } else if (joystickMode == Vehicle::JoystickModeForce) { - // Send the the force setpoint (local pos sp external message) - float dcm[3][3]; - mavlink_euler_to_dcm(roll, pitch, yaw, dcm); - const float fx = -dcm[0][2] * thrust; - const float fy = -dcm[1][2] * thrust; - const float fz = -dcm[2][2] * thrust; - uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else) - mavlink_msg_set_position_target_local_ned_pack_chan( - mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - 0, - 0, - 0, - fx, - fy, - fz, - 0, - 0); - } else if (joystickMode == Vehicle::JoystickModeVelocity) { - // Send the the local velocity setpoint (local pos sp external message) - static float vx = 0; - static float vy = 0; - static float vz = 0; - static float yawrate = 0; - //XXX: find decent scaling - vx -= pitch; - vy += roll; - vz -= 2.0f*(thrust-0.5); - yawrate += yaw; //XXX: not sure what scale to apply here - uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control - mavlink_msg_set_position_target_local_ned_pack_chan( - mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - vx, - vy, - vz, - 0, - 0, - 0, - 0, - yawrate); - } else if (joystickMode == Vehicle::JoystickModeRC) { - // Store scaling values for all 3 axes - static const float axesScaling = 1.0 * 1000.0; - // Calculate the new commands for roll, pitch, yaw, and thrust - const float newRollCommand = roll * axesScaling; - // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward - const float newPitchCommand = -pitch * axesScaling; - const float newYawCommand = yaw * axesScaling; - const float newThrustCommand = thrust * axesScaling; - // Send the MANUAL_COMMAND message - mavlink_msg_manual_control_pack_chan( - static_cast(mavlink->getSystemId()), - static_cast(mavlink->getComponentId()), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - static_cast(this->uasId), - static_cast(newPitchCommand), - static_cast(newRollCommand), - static_cast(newThrustCommand), - static_cast(newYawCommand), - buttons); - } - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); -} - -#ifndef __mobile__ -void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) -{ - if (!_vehicle) { - return; - } - const uint8_t base_mode = _vehicle->baseMode(); - - // If system has manual inputs enabled and is armed - if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) - { - mavlink_message_t message; - float q[4]; - mavlink_euler_to_quaternion(roll, pitch, yaw, q); - - float yawrate = 0.0f; - - // Do not control rates and throttle - quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates - mask |= (1 << 6); // ignore throttle - mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, - QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), - mask, q, 0, 0, 0, 0); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); - quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) | - (1 << 6) | (1 << 7) | (1 << 8); - mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), - MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); - qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; - } - else - { - qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; - } -} -#endif - /** * Order the robot to start receiver pairing */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 9a38b6d59e93afc78de343d89bac4c01996c6bd4..ec6915431577f8410bdaf15368b31ec2764f1b4d 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -193,14 +193,6 @@ public slots: /** @brief Order the robot to pair its receiver **/ void pairRX(int rxType, int rxSubType); - /** @brief Set the values for the manual control of the vehicle */ - void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); - - /** @brief Set the values for the 6dof manual control of the vehicle */ -#ifndef __mobile__ - void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw); -#endif - /** @brief Receive a message from one of the communication links. */ virtual void receiveMessage(mavlink_message_t message);