Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
ccedbbe5
Unverified
Commit
ccedbbe5
authored
Jun 14, 2020
by
Don Gagne
Committed by
GitHub
Jun 14, 2020
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #8835 from DonLakeFlyer/VehicleSendMessage
LinkInterface/Vehicle uses mutexes to synchronize writing bytes
parents
ab442567
72f342d7
Changes
32
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
32 changed files
with
137 additions
and
420 deletions
+137
-420
LogDownloadController.cc
src/AnalyzeView/LogDownloadController.cc
+3
-3
MavlinkConsoleController.cc
src/AnalyzeView/MavlinkConsoleController.cc
+1
-1
APMSensorsComponentController.cc
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
+1
-1
QGCCameraControl.cc
src/Camera/QGCCameraControl.cc
+1
-1
QGCCameraIO.cc
src/Camera/QGCCameraIO.cc
+2
-2
ParameterManager.cc
src/FactSystem/ParameterManager.cc
+4
-4
APMFirmwarePlugin.cc
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+7
-7
APMFirmwarePlugin.h
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+4
-2
FirmwarePlugin.cc
src/FirmwarePlugin/FirmwarePlugin.cc
+2
-5
FirmwarePlugin.h
src/FirmwarePlugin/FirmwarePlugin.h
+4
-1
VirtualJoystick.qml
src/FlightDisplay/VirtualJoystick.qml
+1
-1
RTCMMavlink.cc
src/GPS/RTCM/RTCMMavlink.cc
+1
-1
Joystick.cc
src/Joystick/Joystick.cc
+6
-18
Joystick.h
src/Joystick/Joystick.h
+1
-11
MissionManager.cc
src/MissionManager/MissionManager.cc
+1
-1
PlanManager.cc
src/MissionManager/PlanManager.cc
+6
-6
MultiVehicleManager.cc
src/Vehicle/MultiVehicleManager.cc
+1
-2
TerrainProtocolHandler.cc
src/Vehicle/TerrainProtocolHandler.cc
+1
-1
Vehicle.cc
src/Vehicle/Vehicle.cc
+49
-74
Vehicle.h
src/Vehicle/Vehicle.h
+4
-30
JoystickConfigAdvanced.qml
src/VehicleSetup/JoystickConfigAdvanced.qml
+0
-16
JoystickConfigGeneral.qml
src/VehicleSetup/JoystickConfigGeneral.qml
+1
-1
LinkInterface.cc
src/comm/LinkInterface.cc
+8
-1
LinkInterface.h
src/comm/LinkInterface.h
+8
-24
LinkManager.cc
src/comm/LinkManager.cc
+1
-1
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+1
-1
MockLink.cc
src/comm/MockLink.cc
+9
-1
MockLink.h
src/comm/MockLink.h
+5
-1
TCPLinkTest.cc
src/qgcunittest/TCPLinkTest.cc
+1
-1
FileManager.cc
src/uas/FileManager.cc
+1
-1
UAS.cc
src/uas/UAS.cc
+2
-192
UAS.h
src/uas/UAS.h
+0
-8
No files found.
src/AnalyzeView/LogDownloadController.cc
View file @
ccedbbe5
...
...
@@ -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
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
refresh
();
}
}
...
...
src/AnalyzeView/MavlinkConsoleController.cc
View file @
ccedbbe5
...
...
@@ -129,7 +129,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
0
,
chunk
.
size
(),
reinterpret_cast
<
uint8_t
*>
(
chunk
.
data
()));
_vehicle
->
sendMessageOnLink
(
priority_link
,
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
priority_link
,
msg
);
data
.
remove
(
0
,
chunk
.
size
());
}
}
...
...
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file @
ccedbbe5
...
...
@@ -704,7 +704,7 @@ void APMSensorsComponentController::nextClicked(void)
0
,
// target_system
0
);
// target_component
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
if
(
_calTypeInProgress
==
CalTypeCompassMot
)
{
_stopCalibration
(
StopCalibrationSuccess
);
...
...
src/Camera/QGCCameraControl.cc
View file @
ccedbbe5
...
...
@@ -1183,7 +1183,7 @@ QGCCameraControl::_requestAllParameters()
&
msg
,
static_cast
<
uint8_t
>
(
_vehicle
->
id
()),
static_cast
<
uint8_t
>
(
compID
()));
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
qCDebug
(
CameraControlVerboseLog
)
<<
"Request all parameters"
;
}
...
...
src/Camera/QGCCameraIO.cc
View file @
ccedbbe5
...
...
@@ -191,7 +191,7 @@ QGCCameraParamIO::_sendParameter()
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
&
p
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
_paramWriteTimer
.
start
();
}
...
...
@@ -364,6 +364,6 @@ QGCCameraParamIO::paramRequest(bool reset)
static_cast
<
uint8_t
>
(
_control
->
compID
()),
param_id
,
-
1
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
_paramRequestTimer
.
start
();
}
src/FactSystem/ParameterManager.cc
View file @
ccedbbe5
...
...
@@ -526,7 +526,7 @@ void ParameterManager::refreshAllParameters(uint8_t componentId)
&
msg
,
_vehicle
->
id
(),
componentId
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
// Give the user some feedback things loaded properly
QVariantAnimation
*
ani
=
new
QVariantAnimation
(
this
);
...
...
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @
ccedbbe5
...
...
@@ -289,10 +289,8 @@ void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_mess
&
paramValue
);
}
void
APMFirmwarePlugin
::
_handleOutgoingParamSet
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
void
APMFirmwarePlugin
::
_handleOutgoingParamSet
ThreadSafe
(
Vehicle
*
/*vehicle*/
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
{
Q_UNUSED
(
vehicle
);
mavlink_param_set_t
paramSet
;
mavlink_param_union_t
paramUnion
;
...
...
@@ -336,7 +334,9 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
qCCritical
(
APMFirmwarePluginLog
)
<<
"Invalid/Unsupported data type used in parameter:"
<<
paramSet
.
param_type
;
}
_adjustOutgoingMavlinkMutex
.
lock
();
mavlink_msg_param_set_encode_chan
(
message
->
sysid
,
message
->
compid
,
outgoingLink
->
mavlinkChannel
(),
message
,
&
paramSet
);
_adjustOutgoingMavlinkMutex
.
unlock
();
}
bool
APMFirmwarePlugin
::
_handleIncomingStatusText
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
...
...
@@ -508,11 +508,11 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
return
true
;
}
void
APMFirmwarePlugin
::
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
void
APMFirmwarePlugin
::
adjustOutgoingMavlinkMessage
ThreadSafe
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
{
switch
(
message
->
msgid
)
{
case
MAVLINK_MSG_ID_PARAM_SET
:
_handleOutgoingParamSet
(
vehicle
,
outgoingLink
,
message
);
_handleOutgoingParamSet
ThreadSafe
(
vehicle
,
outgoingLink
,
message
);
break
;
}
}
...
...
@@ -945,7 +945,7 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
&
msg
,
&
cmd
);
vehicle
->
sendMessageOnLink
(
vehicle
->
priorityLink
(),
msg
);
vehicle
->
sendMessageOnLink
ThreadSafe
(
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
->
sendMessageOnLink
ThreadSafe
(
vehicle
->
priorityLink
(),
message
);
}
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @
ccedbbe5
...
...
@@ -93,7 +93,7 @@ public:
void
guidedModeRTL
(
Vehicle
*
vehicle
,
bool
smartRTL
)
override
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
override
;
bool
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
override
;
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
override
;
void
adjustOutgoingMavlinkMessage
ThreadSafe
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
override
;
virtual
void
initializeStreamRates
(
Vehicle
*
vehicle
);
void
initializeVehicle
(
Vehicle
*
vehicle
)
override
;
bool
sendHomePositionToVehicle
(
void
)
override
;
...
...
@@ -127,7 +127,7 @@ private:
void
_handleIncomingParamValue
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
bool
_handleIncomingStatusText
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
void
_handleIncomingHeartbeat
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
void
_handleOutgoingParamSet
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
);
void
_handleOutgoingParamSet
ThreadSafe
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
);
void
_soloVideoHandshake
(
Vehicle
*
vehicle
,
bool
originalSoloFirmware
);
bool
_guidedModeTakeoff
(
Vehicle
*
vehicle
,
double
altitudeRel
);
void
_handleRCChannels
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
...
...
@@ -141,6 +141,8 @@ private:
QList
<
APMCustomMode
>
_supportedModes
;
QMap
<
int
/* vehicle id */
,
QMap
<
int
/* componentId */
,
bool
/* true: component is part of ArduPilot stack */
>>
_ardupilotComponentMap
;
QMutex
_adjustOutgoingMavlinkMutex
;
static
const
char
*
_artooIP
;
static
const
int
_artooVideoHandshakePort
;
};
...
...
src/FirmwarePlugin/FirmwarePlugin.cc
View file @
ccedbbe5
...
...
@@ -156,11 +156,8 @@ bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_mess
return
true
;
}
void
FirmwarePlugin
::
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
void
FirmwarePlugin
::
adjustOutgoingMavlinkMessage
ThreadSafe
(
Vehicle
*
/*vehicle*/
,
LinkInterface
*
/*outgoingLink*/
,
mavlink_message_t
*
/*message*/
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
outgoingLink
);
Q_UNUSED
(
message
);
// Generic plugin does no message adjustment
}
...
...
@@ -947,5 +944,5 @@ void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionRe
vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
message
,
&
follow_target
);
vehicle
->
sendMessageOnLink
(
vehicle
->
priorityLink
(),
message
);
vehicle
->
sendMessageOnLink
ThreadSafe
(
vehicle
->
priorityLink
(),
message
);
}
src/FirmwarePlugin/FirmwarePlugin.h
View file @
ccedbbe5
...
...
@@ -193,10 +193,13 @@ public:
/// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics.
/// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain
/// mavlink generic.
///
/// This method must be thread safe.
///
/// @param vehicle Vehicle message came from
/// @param outgoingLink Link that messae is going out on
/// @param message[in,out] Mavlink message to adjust if needed.
virtual
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
);
virtual
void
adjustOutgoingMavlinkMessage
ThreadSafe
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
);
/// Determines how to handle the first item of the mission item list. Internally to QGC the first item
/// is always the home position.
...
...
src/FlightDisplay/VirtualJoystick.qml
View file @
ccedbbe5
...
...
@@ -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
)
}
}
}
...
...
src/GPS/RTCM/RTCMMavlink.cc
View file @
ccedbbe5
...
...
@@ -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
->
sendMessageOnLink
ThreadSafe
(
vehicle
->
priorityLink
(),
message
);
}
}
src/Joystick/Joystick.cc
View file @
ccedbbe5
...
...
@@ -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
<
uint16_t
>
(
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
;
}
}
...
...
src/Joystick/Joystick.h
View file @
ccedbbe5
...
...
@@ -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
();
...
...
src/MissionManager/MissionManager.cc
View file @
ccedbbe5
...
...
@@ -69,7 +69,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
&
messageOut
,
&
missionItem
);
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
messageOut
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_dedicatedLink
,
messageOut
);
_startAckTimeout
(
AckGuidedItem
);
emit
inProgressChanged
(
true
);
}
...
...
src/MissionManager/PlanManager.cc
View file @
ccedbbe5
...
...
@@ -119,7 +119,7 @@ void PlanManager::_writeMissionCount(void)
_writeMissionItems
.
count
(),
_planType
);
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
message
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_dedicatedLink
,
message
);
_startAckTimeout
(
AckMissionRequest
);
}
...
...
@@ -161,7 +161,7 @@ void PlanManager::_requestList(void)
MAV_COMP_ID_AUTOPILOT1
,
_planType
);
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
message
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_dedicatedLink
,
message
);
_startAckTimeout
(
AckMissionCount
);
}
...
...
@@ -301,7 +301,7 @@ void PlanManager::_readTransactionComplete(void)
MAV_MISSION_ACCEPTED
,
_planType
);
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
message
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_dedicatedLink
,
message
);
_finishTransaction
(
true
);
}
...
...
@@ -369,7 +369,7 @@ void PlanManager::_requestNextMissionItem(void)
_planType
);
}
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
message
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_dedicatedLink
,
message
);
_startAckTimeout
(
AckMissionItem
);
}
...
...
@@ -586,7 +586,7 @@ void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool m
_planType
);
}
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
messageOut
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_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
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
message
);
_startAckTimeout
(
AckMissionClearAll
);
}
...
...
src/Vehicle/MultiVehicleManager.cc
View file @
ccedbbe5
...
...
@@ -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
);
}
}
}
...
...
src/Vehicle/TerrainProtocolHandler.cc
View file @
ccedbbe5
...
...
@@ -146,7 +146,7 @@ void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate& swCorner, ui
_currentTerrainRequest
.
grid_spacing
,
gridBit
,
terrainData
);
_vehicle
->
sendMessageOnLink
(
_vehicle
->
priorityLink
(),
msg
);
_vehicle
->
sendMessageOnLink
ThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
}
}
}
src/Vehicle/Vehicle.cc
View file @
ccedbbe5
This diff is collapsed.
Click to expand it.
src/Vehicle/Vehicle.h
View file @
ccedbbe5
...
...
@@ -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
sendMessageOnLink
ThreadSafe
(
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
<
LinkInterface
*>
_links
;
JoystickMode_t
_joystickMode
;
bool
_joystickEnabled
;
UAS
*
_uas
;
...
...
@@ -1648,7 +1624,5 @@ private:
// Settings keys
static
const
char
*
_settingsGroup
;
static
const
char
*
_joystickModeSettingsKey
;
static
const
char
*
_joystickEnabledSettingsKey
;
};
src/VehicleSetup/JoystickConfigAdvanced.qml
View file @
ccedbbe5
...
...
@@ -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):
"
)
...
...
src/VehicleSetup/JoystickConfigGeneral.qml
View file @
ccedbbe5
...
...
@@ -200,7 +200,7 @@ Item {
Connections
{
target
:
_activeJoystick
on
ManualControl
:
{
on
AxisValues
:
{
rollAxis
.
axisValue
=
roll
*
32768.0
pitchAxis
.
axisValue
=
pitch
*
32768.0
yawAxis
.
axisValue
=
yaw
*
32768.0
...
...
src/comm/LinkInterface.cc
View file @
ccedbbe5
...
...
@@ -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
*>
(
"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
();
}
src/comm/LinkInterface.h
View file @
ccedbbe5
...
...
@@ -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
...
...
src/comm/LinkManager.cc
View file @
ccedbbe5
...
...
@@ -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
->
writeBytes
Thread
Safe
(
"
\r
"
,
1
);
QSignalSpy
spy
(
link
,
SIGNAL
(
bytesReceived
(
LinkInterface
*
,
QByteArray
)));
if
(
spy
.
wait
(
100
))
{
QList
<
QVariant
>
arguments
=
spy
.
takeFirst
();
...
...
src/comm/MAVLinkProtocol.cc
View file @
ccedbbe5
...
...
@@ -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
->
writeBytes
Thread
Safe
((
const
char
*
)
buf
,
len
);
}
}
...
...
src/comm/MockLink.cc
View file @
ccedbbe5
...
...
@@ -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
...
...
src/comm/MockLink.h
View file @
ccedbbe5
...
...
@@ -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
);
...
...
src/qgcunittest/TCPLinkTest.cc
View file @
ccedbbe5
...
...
@@ -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
->
writeBytes
Thread
Safe
(
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
...
...
src/uas/FileManager.cc