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
367b0b85
Commit
367b0b85
authored
Mar 09, 2016
by
Don Gagne
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Add Guided mode support
parent
cd39a194
Changes
16
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
16 changed files
with
1030 additions
and
296 deletions
+1030
-296
APMFirmwarePlugin.cc
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+238
-173
APMFirmwarePlugin.h
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+10
-3
ArduCopterFirmwarePlugin.cc
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+103
-0
ArduCopterFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+11
-0
FirmwarePlugin.cc
src/FirmwarePlugin/FirmwarePlugin.cc
+82
-3
FirmwarePlugin.h
src/FirmwarePlugin/FirmwarePlugin.h
+44
-6
PX4FirmwarePlugin.cc
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+8
-10
PX4FirmwarePlugin.h
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+2
-2
FlightDisplayView.qml
src/FlightDisplay/FlightDisplayView.qml
+13
-14
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+45
-17
FlightDisplayViewWidgets.qml
src/FlightDisplay/FlightDisplayViewWidgets.qml
+233
-4
MissionManager.cc
src/MissionManager/MissionManager.cc
+51
-6
MissionManager.h
src/MissionManager/MissionManager.h
+6
-0
Vehicle.cc
src/Vehicle/Vehicle.cc
+133
-6
Vehicle.h
src/Vehicle/Vehicle.h
+51
-5
MainToolBarIndicators.qml
src/ui/toolbar/MainToolBarIndicators.qml
+0
-47
No files found.
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @
367b0b85
This diff is collapsed.
Click to expand it.
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @
367b0b85
...
...
@@ -84,12 +84,15 @@ public:
QList
<
VehicleComponent
*>
componentsForVehicle
(
AutoPilotPlugin
*
vehicle
)
final
;
QList
<
MAV_CMD
>
supportedMissionCommands
(
void
)
final
;
bool
isCapable
(
FirmwareCapabilities
capabilities
)
final
;
bool
isCapable
(
FirmwareCapabilities
capabilities
);
QStringList
flightModes
(
void
)
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
final
;
bool
setFlightMode
(
const
QString
&
flightMode
,
uint8_t
*
base_mode
,
uint32_t
*
custom_mode
)
final
;
bool
isGuidedMode
(
const
Vehicle
*
vehicle
)
const
final
;
void
pauseVehicle
(
Vehicle
*
vehicle
);
int
manualControlReservedButtonCount
(
void
)
final
;
void
adjustMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
initializeVehicle
(
Vehicle
*
vehicle
)
final
;
bool
sendHomePositionToVehicle
(
void
)
final
;
void
addMetaDataToFact
(
QObject
*
parameterMetaData
,
Fact
*
fact
,
MAV_TYPE
vehicleType
)
final
;
...
...
@@ -111,6 +114,10 @@ private:
static
bool
_isTextSeverityAdjustmentNeeded
(
const
APMFirmwareVersion
&
firmwareVersion
);
void
_setInfoSeverity
(
mavlink_message_t
*
message
)
const
;
QString
_getMessageText
(
mavlink_message_t
*
message
)
const
;
void
_handleParamValue
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
void
_handleParamSet
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
void
_handleStatusText
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
void
_handleHeartbeat
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
APMFirmwareVersion
_firmwareVersion
;
bool
_textSeverityAdjustmentNeeded
;
...
...
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @
367b0b85
...
...
@@ -25,6 +25,8 @@
/// @author Don Gagne <don@thegagnes.com>
#include "ArduCopterFirmwarePlugin.h"
#include "QGCApplication.h"
#include "MissionManager.h"
APMCopterMode
::
APMCopterMode
(
uint32_t
mode
,
bool
settable
)
:
APMCustomMode
(
mode
,
settable
)
...
...
@@ -73,3 +75,104 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
supportedFlightModes
<<
APMCopterMode
(
APMCopterMode
::
BRAKE
,
true
);
setSupportedModes
(
supportedFlightModes
);
}
bool
ArduCopterFirmwarePlugin
::
isCapable
(
FirmwareCapabilities
capabilities
)
{
return
(
capabilities
&
(
SetFlightModeCapability
|
GuidedModeCapability
|
PauseVehicleCapability
))
==
capabilities
;
}
void
ArduCopterFirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
{
vehicle
->
setFlightMode
(
"RTL"
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeLand
(
Vehicle
*
vehicle
)
{
vehicle
->
setFlightMode
(
"Land"
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeTakeoff
(
Vehicle
*
vehicle
,
double
altitudeRel
)
{
if
(
qIsNaN
(
vehicle
->
altitudeAMSL
()
->
rawValue
().
toDouble
()))
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Unable to takeoff, vehicle position not known."
));
return
;
}
mavlink_message_t
msg
;
mavlink_command_long_t
cmd
;
cmd
.
command
=
(
uint16_t
)
MAV_CMD_NAV_TAKEOFF
;
cmd
.
confirmation
=
0
;
cmd
.
param1
=
0.0
f
;
cmd
.
param2
=
0.0
f
;
cmd
.
param3
=
0.0
f
;
cmd
.
param4
=
0.0
f
;
cmd
.
param5
=
0.0
f
;
cmd
.
param6
=
0.0
f
;
cmd
.
param7
=
vehicle
->
altitudeAMSL
()
->
rawValue
().
toFloat
()
+
altitudeRel
;
// AMSL meters
cmd
.
target_system
=
vehicle
->
id
();
cmd
.
target_component
=
0
;
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
mavlink_msg_command_long_encode
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
msg
,
&
cmd
);
vehicle
->
sendMessage
(
msg
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
{
if
(
qIsNaN
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
()))
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Unable to go to location, vehicle position not known."
));
return
;
}
vehicle
->
setFlightMode
(
"Guided"
);
QGeoCoordinate
coordWithAltitude
=
gotoCoord
;
coordWithAltitude
.
setAltitude
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
());
vehicle
->
missionManager
()
->
writeArduPilotGuidedMissionItem
(
coordWithAltitude
,
false
/* altChangeOnly */
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
)
{
if
(
qIsNaN
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
()))
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Unable to change altitude, vehicle altitude not known."
));
return
;
}
mavlink_message_t
msg
;
mavlink_set_position_target_local_ned_t
cmd
;
memset
(
&
cmd
,
0
,
sizeof
(
mavlink_set_position_target_local_ned_t
));
cmd
.
target_system
=
vehicle
->
id
();
cmd
.
target_component
=
0
;
cmd
.
coordinate_frame
=
MAV_FRAME_LOCAL_OFFSET_NED
;
cmd
.
type_mask
=
0xFFF8
;
// Only x/y/z valid
cmd
.
x
=
0.0
f
;
cmd
.
y
=
0.0
f
;
cmd
.
z
=
-
(
altitudeRel
-
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
());
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
mavlink_msg_set_position_target_local_ned_encode
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
&
msg
,
&
cmd
);
vehicle
->
sendMessage
(
msg
);
}
bool
ArduCopterFirmwarePlugin
::
isPaused
(
const
Vehicle
*
vehicle
)
const
{
return
vehicle
->
flightMode
()
==
"Brake"
;
}
void
ArduCopterFirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
vehicle
->
setFlightMode
(
"Brake"
);
}
void
ArduCopterFirmwarePlugin
::
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
{
if
(
guidedMode
)
{
vehicle
->
setFlightMode
(
"Guided"
);
}
else
{
pauseVehicle
(
vehicle
);
}
}
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @
367b0b85
...
...
@@ -64,6 +64,17 @@ class ArduCopterFirmwarePlugin : public APMFirmwarePlugin
public:
ArduCopterFirmwarePlugin
(
void
);
// Overrides from FirmwarePlugin
bool
isCapable
(
FirmwareCapabilities
capabilities
)
final
;
bool
isPaused
(
const
Vehicle
*
vehicle
)
const
final
;
void
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
final
;
void
pauseVehicle
(
Vehicle
*
vehicle
)
final
;
void
guidedModeRTL
(
Vehicle
*
vehicle
)
final
;
void
guidedModeLand
(
Vehicle
*
vehicle
)
final
;
void
guidedModeTakeoff
(
Vehicle
*
vehicle
,
double
altitudeRel
)
final
;
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
final
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
)
final
;
};
#endif
src/FirmwarePlugin/FirmwarePlugin.cc
View file @
367b0b85
...
...
@@ -22,9 +22,16 @@
======================================================================*/
#include "FirmwarePlugin.h"
#include "QGCApplication.h"
#include <QDebug>
bool
FirmwarePlugin
::
isCapable
(
FirmwareCapabilities
capabilities
)
{
Q_UNUSED
(
capabilities
);
return
false
;
}
QList
<
VehicleComponent
*>
FirmwarePlugin
::
componentsForVehicle
(
AutoPilotPlugin
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
...
...
@@ -32,7 +39,7 @@ QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* v
return
QList
<
VehicleComponent
*>
();
}
QString
FirmwarePlugin
::
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
QString
FirmwarePlugin
::
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
{
QString
flightMode
;
...
...
@@ -86,11 +93,17 @@ int FirmwarePlugin::manualControlReservedButtonCount(void)
return
-
1
;
}
void
FirmwarePlugin
::
adjustMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
void
FirmwarePlugin
::
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
message
);
// Generic plugin does no message adjustment
}
void
FirmwarePlugin
::
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
message
);
// Generic plugin does no message adjustment
}
...
...
@@ -129,3 +142,69 @@ void FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataFile
majorVersion
=
-
1
;
minorVersion
=
-
1
;
}
bool
FirmwarePlugin
::
isGuidedMode
(
const
Vehicle
*
vehicle
)
const
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
return
false
;
}
void
FirmwarePlugin
::
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
guidedMode
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
bool
FirmwarePlugin
::
isPaused
(
const
Vehicle
*
vehicle
)
const
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
return
false
;
}
void
FirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
void
FirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
void
FirmwarePlugin
::
guidedModeLand
(
Vehicle
*
vehicle
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
void
FirmwarePlugin
::
guidedModeTakeoff
(
Vehicle
*
vehicle
,
double
altitudeRel
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
altitudeRel
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
void
FirmwarePlugin
::
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
gotoCoord
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
void
FirmwarePlugin
::
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
)
{
// Not supported by generic vehicle
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
altitudeRel
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Guided mode not supported by Vehicle."
));
}
src/FirmwarePlugin/FirmwarePlugin.h
View file @
367b0b85
...
...
@@ -50,12 +50,14 @@ class FirmwarePlugin : public QObject
public:
/// Set of optional capabilites which firmware may support
typedef
enum
{
SetFlightModeCapability
,
///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability
,
///< MAV_CMD_PREFLIGHT_STORAGE is supported
SetFlightModeCapability
=
1
<<
0
,
///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability
=
1
<<
1
,
///< MAV_CMD_PREFLIGHT_STORAGE is supported
PauseVehicleCapability
=
1
<<
2
,
///< Vehicle supports pausing at current location
GuidedModeCapability
=
1
<<
3
,
///< Vehicle Support guided mode commands
}
FirmwareCapabilities
;
/// @return true: Firmware supports all specified capabilites
virtual
bool
isCapable
(
FirmwareCapabilities
capabilities
)
{
Q_UNUSED
(
capabilities
);
return
false
;
}
virtual
bool
isCapable
(
FirmwareCapabilities
capabilities
)
;
/// Returns VehicleComponents for specified Vehicle
/// @param vehicle Vehicle to associate with components
...
...
@@ -69,13 +71,42 @@ public:
/// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable.
/// @param base_mode Base mode from mavlink HEARTBEAT message
/// @param custom_mode Custom mode from mavlink HEARTBEAT message
virtual
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
);
virtual
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
;
/// Sets base_mode and custom_mode to specified flight mode.
/// @param[out] base_mode Base mode for SET_MODE mavlink message
/// @param[out] custom_mode Custom mode for SET_MODE mavlink message
virtual
bool
setFlightMode
(
const
QString
&
flightMode
,
uint8_t
*
base_mode
,
uint32_t
*
custom_mode
);
/// Returns whether the vehicle is in guided mode or not.
virtual
bool
isGuidedMode
(
const
Vehicle
*
vehicle
)
const
;
/// Set guided flight mode
virtual
void
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
);
/// Returns whether the vehicle is paused or not.
virtual
bool
isPaused
(
const
Vehicle
*
vehicle
)
const
;
/// Causes the vehicle to stop at current position. If guide mode is supported, vehicle will be let in guide mode.
/// If not, vehicle will be left in Loiter.
virtual
void
pauseVehicle
(
Vehicle
*
vehicle
);
/// Command vehicle to return to launch
virtual
void
guidedModeRTL
(
Vehicle
*
vehicle
);
/// Command vehicle to land at current location
virtual
void
guidedModeLand
(
Vehicle
*
vehicle
);
/// Command vehicle to takeoff from current location
/// @param altitudeRel Relative altitude to takeoff to
virtual
void
guidedModeTakeoff
(
Vehicle
*
vehicle
,
double
altitudeRel
);
/// Command vehicle to move to specified location (altitude is included and relative)
virtual
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
);
/// Command vehicle to change to the specified relatice altitude
virtual
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeRel
);
/// FIXME: This isn't quite correct being here. All code for Joystick suvehicleTypepport is currently firmware specific
/// not just this. I'm going to try to change that. If not, this will need to be removed.
/// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
...
...
@@ -89,8 +120,15 @@ public:
/// spec implementations such that the base code can remain mavlink generic.
/// @param vehicle Vehicle message came from
/// @param message[in,out] Mavlink message to adjust if needed.
virtual
void
adjustMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
virtual
void
adjust
Incoming
MavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
/// 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.
/// @param vehicle Vehicle message came from
/// @param message[in,out] Mavlink message to adjust if needed.
virtual
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
);
/// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
virtual
void
initializeVehicle
(
Vehicle
*
vehicle
);
...
...
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file @
367b0b85
...
...
@@ -111,7 +111,7 @@ QStringList PX4FirmwarePlugin::flightModes(void)
return
flightModes
;
}
QString
PX4FirmwarePlugin
::
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
QString
PX4FirmwarePlugin
::
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
{
QString
flightMode
=
"Unknown"
;
...
...
@@ -178,17 +178,10 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
return
0
;
// 0 buttons reserved for rc switch simulation
}
void
PX4FirmwarePlugin
::
adjustMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
message
);
// PX4 Flight Stack plugin does no message adjustment
}
bool
PX4FirmwarePlugin
::
isCapable
(
FirmwareCapabilities
capabilities
)
{
return
(
capabilities
&
(
MavCmdPreflightStorageCapability
|
SetFlightModeCapability
))
==
capabilities
;
qDebug
()
<<
(
capabilities
&
(
MavCmdPreflightStorageCapability
|
SetFlightModeCapability
|
PauseVehicleCapability
))
<<
capabilities
;
return
(
capabilities
&
(
MavCmdPreflightStorageCapability
|
SetFlightModeCapability
|
PauseVehicleCapability
))
==
capabilities
;
}
void
PX4FirmwarePlugin
::
initializeVehicle
(
Vehicle
*
vehicle
)
...
...
@@ -247,3 +240,8 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
metaData
->
loadParameterFactMetaDataFile
(
metaDataFile
);
return
metaData
;
}
void
PX4FirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
vehicle
->
setFlightMode
(
"Auto: Pause"
);
}
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @
367b0b85
...
...
@@ -43,10 +43,10 @@ public:
bool
isCapable
(
FirmwareCapabilities
capabilities
)
final
;
QStringList
flightModes
(
void
)
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
final
;
QString
flightMode
(
uint8_t
base_mode
,
uint32_t
custom_mode
)
const
final
;
bool
setFlightMode
(
const
QString
&
flightMode
,
uint8_t
*
base_mode
,
uint32_t
*
custom_mode
)
final
;
void
pauseVehicle
(
Vehicle
*
vehicle
)
final
;
int
manualControlReservedButtonCount
(
void
)
final
;
void
adjustMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
final
;
void
initializeVehicle
(
Vehicle
*
vehicle
)
final
;
bool
sendHomePositionToVehicle
(
void
)
final
;
void
addMetaDataToFact
(
QObject
*
parameterMetaData
,
Fact
*
fact
,
MAV_TYPE
vehicleType
)
final
;
...
...
src/FlightDisplay/FlightDisplayView.qml
View file @
367b0b85
...
...
@@ -47,22 +47,8 @@ QGCView {
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
enabled
}
property
real
availableHeight
:
parent
.
height
readonly
property
bool
isBackgroundDark
:
_mainIsMap
?
(
_flightMap
?
_flightMap
.
isSatelliteMap
:
true
)
:
true
property
var
_activeVehicle
:
multiVehicleManager
.
activeVehicle
readonly
property
real
_defaultRoll
:
0
readonly
property
real
_defaultPitch
:
0
readonly
property
real
_defaultHeading
:
0
readonly
property
real
_defaultAltitudeAMSL
:
0
readonly
property
real
_defaultGroundSpeed
:
0
readonly
property
real
_defaultAirSpeed
:
0
readonly
property
string
_mapName
:
"
FlightDisplayView
"
readonly
property
string
_showMapBackgroundKey
:
"
/showMapBackground
"
readonly
property
string
_mainIsMapKey
:
"
MainFlyWindowIsMap
"
readonly
property
string
_PIPVisibleKey
:
"
IsPIPVisible
"
property
bool
_mainIsMap
:
QGroundControl
.
loadBoolGlobalSetting
(
_mainIsMapKey
,
true
)
property
bool
_isPipVisible
:
QGroundControl
.
loadBoolGlobalSetting
(
_PIPVisibleKey
,
true
)
...
...
@@ -82,6 +68,18 @@ QGCView {
property
real
pipSize
:
mainWindow
.
width
*
0.2
readonly
property
bool
isBackgroundDark
:
_mainIsMap
?
(
_flightMap
?
_flightMap
.
isSatelliteMap
:
true
)
:
true
readonly
property
real
_defaultRoll
:
0
readonly
property
real
_defaultPitch
:
0
readonly
property
real
_defaultHeading
:
0
readonly
property
real
_defaultAltitudeAMSL
:
0
readonly
property
real
_defaultGroundSpeed
:
0
readonly
property
real
_defaultAirSpeed
:
0
readonly
property
string
_mapName
:
"
FlightDisplayView
"
readonly
property
string
_showMapBackgroundKey
:
"
/showMapBackground
"
readonly
property
string
_mainIsMapKey
:
"
MainFlyWindowIsMap
"
readonly
property
string
_PIPVisibleKey
:
"
IsPIPVisible
"
FlightDisplayViewController
{
id
:
_controller
}
function
setStates
()
{
...
...
@@ -174,6 +172,7 @@ QGCView {
FlightDisplayViewMap
{
id
:
_flightMap
anchors.fill
:
parent
flightWidgets
:
widgetsLoader
.
item
}
}
...
...
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
367b0b85
...
...
@@ -40,9 +40,14 @@ FlightMap {
anchors.fill
:
parent
mapName
:
_mapName
property
alias
missionController
:
_missionController
property
var
flightWidgets
property
bool
_followVehicle
:
true
property
bool
_activeVehicleCoordinateValid
:
multiVehicleManager
.
activeVehicle
?
multiVehicleManager
.
activeVehicle
.
coordinateValid
:
false
property
var
activeVehicleCoordinate
:
multiVehicleManager
.
activeVehicle
?
multiVehicleManager
.
activeVehicle
.
coordinate
:
QtPositioning
.
coordinate
()
property
var
_activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
property
bool
_activeVehicleCoordinateValid
:
_activeVehicle
?
_activeVehicle
.
coordinateValid
:
false
property
var
activeVehicleCoordinate
:
_activeVehicle
?
_activeVehicle
.
coordinate
:
QtPositioning
.
coordinate
()
property
var
_gotoHereCoordinate
:
QtPositioning
.
coordinate
()
Component.onCompleted
:
{
QGroundControl
.
flightMapPosition
=
center
...
...
@@ -58,6 +63,8 @@ FlightMap {
}
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
MissionController
{
id
:
_missionController
Component.onCompleted
:
start
(
false
/* editMode */
)
...
...
@@ -68,14 +75,14 @@ FlightMap {
model
:
_mainIsMap
?
multiVehicleManager
.
activeVehicle
?
multiVehicleManager
.
activeVehicle
.
trajectoryPoints
:
0
:
0
delegate
:
MapPolyline
{
line.width
:
3
line.color
:
"
red
"
z
:
QGroundControl
.
zOrderMapItems
-
1
path
:
[
{
latitude
:
object
.
coordinate1
.
latitude
,
longitude
:
object
.
coordinate1
.
longitude
},
{
latitude
:
object
.
coordinate2
.
latitude
,
longitude
:
object
.
coordinate2
.
longitude
},
]
}
line.width
:
3
line.color
:
"
red
"
z
:
QGroundControl
.
zOrderMapItems
-
1
path
:
[
{
latitude
:
object
.
coordinate1
.
latitude
,
longitude
:
object
.
coordinate1
.
longitude
},
{
latitude
:
object
.
coordinate2
.
latitude
,
longitude
:
object
.
coordinate2
.
longitude
},
]
}
}
// Add the vehicles to the map
...
...
@@ -83,12 +90,12 @@ FlightMap {
model
:
multiVehicleManager
.
vehicles
delegate
:
VehicleMapItem
{
vehicle
:
object
coordinate
:
object
.
coordinate
isSatellite
:
flightMap
.
isSatelliteMap
size
:
_mainIsMap
?
ScreenTools
.
defaultFontPixelHeight
*
5
:
ScreenTools
.
defaultFontPixelHeight
*
2
z
:
QGroundControl
.
zOrderMapItems
}
vehicle
:
object
coordinate
:
object
.
coordinate
isSatellite
:
flightMap
.
isSatelliteMap
size
:
_mainIsMap
?
ScreenTools
.
defaultFontPixelHeight
*
5
:
ScreenTools
.
defaultFontPixelHeight
*
2
z
:
QGroundControl
.
zOrderMapItems
}
}
// Add the mission items to the map
...
...
@@ -101,8 +108,29 @@ FlightMap {
model
:
_mainIsMap
?
_missionController
.
waypointLines
:
0
}
// Used to make pinch zoom work
// GoTo here waypoint
MapQuickItem
{
coordinate
:
_gotoHereCoordinate
visible
:
_vehicle
.
guidedMode
&&
_gotoHereCoordinate
.
isValid
z
:
QGroundControl
.
zOrderMapItems
anchorPoint.x
:
sourceItem
.
width
/
2
anchorPoint.y
:
sourceItem
.
height
/
2
sourceItem
:
MissionItemIndexLabel
{
isCurrentItem
:
true
label
:
"
G
"
}
}
// Handle guided mode clicks
MouseArea
{
anchors.fill
:
parent
onClicked
:
{
if
(
_activeVehicle
&&
_activeVehicle
.
guidedMode
)
{
_gotoHereCoordinate
=
flightMap
.
toCoordinate
(
Qt
.
point
(
mouse
.
x
,
mouse
.
y
))
flightWidgets
.
guidedModeBar
.
confirmAction
(
flightWidgets
.
guidedModeBar
.
confirmGoTo
)
}
}
}
}
src/FlightDisplay/FlightDisplayViewWidgets.qml
View file @
367b0b85
This diff is collapsed.
Click to expand it.
src/MissionManager/MissionManager.cc
View file @
367b0b85
...
...
@@ -107,6 +107,41 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
emit
inProgressChanged
(
true
);
}
void
MissionManager
::
writeArduPilotGuidedMissionItem
(
const
QGeoCoordinate
&
gotoCoord
,
bool
altChangeOnly
)
{
if
(
inProgress
())
{
qCDebug
(
MissionManagerLog
)
<<
"writeArduPilotGuidedMissionItem called while transaction in progress"
;
return
;
}
_writeTransactionInProgress
=
true
;
mavlink_message_t
messageOut
;
mavlink_mission_item_t
missionItem
;
missionItem
.
target_system
=
_vehicle
->
id
();
missionItem
.
target_component
=
0
;
missionItem
.
seq
=
0
;
missionItem
.
command
=
MAV_CMD_NAV_WAYPOINT
;
missionItem
.
param1
=
0
;
missionItem
.
param2
=
0
;
missionItem
.
param3
=
0
;
missionItem
.
param4
=
0
;
missionItem
.
x
=
gotoCoord
.
latitude
();
missionItem
.
y
=
gotoCoord
.
longitude
();
missionItem
.
z
=
gotoCoord
.
altitude
();
missionItem
.
frame
=
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
missionItem
.
current
=
altChangeOnly
?
3
:
2
;
missionItem
.
autocontinue
=
true
;
mavlink_msg_mission_item_encode
(
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
()
->
getSystemId
(),
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
()
->
getComponentId
(),
&
messageOut
,
&
missionItem
);
_dedicatedLink
=
_vehicle
->
priorityLink
();
_vehicle
->
sendMessageOnLink
(
_dedicatedLink
,
messageOut
);
_startAckTimeout
(
AckGuidedItem
);
emit
inProgressChanged
(
true
);
}
void
MissionManager
::
requestMissionItems
(
void
)
{
qCDebug
(
MissionManagerLog
)
<<
"requestMissionItems read sequence"
;
...
...
@@ -211,8 +246,6 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
}
_requestNextMissionItem
();
}
}
void
MissionManager
::
_requestNextMissionItem
(
void
)
...
...
@@ -394,6 +427,16 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
_finishTransaction
(
false
);
}
break
;
case
AckGuidedItem
:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if
(
missionAck
.
type
==
MAV_MISSION_ACCEPTED
)
{
qCDebug
(
MissionManagerLog
)
<<
"_handleMissionAck guide mode item accepted"
;
_finishTransaction
(
true
);
}
else
{
_sendError
(
VehicleError
,
QString
(
"Vehicle returned error: %1. Vehicle did not accept guided item."
).
arg
(
_missionResultToString
((
MAV_MISSION_RESULT
)
missionAck
.
type
)));
_finishTransaction
(
false
);
}
break
;
}
}
...
...
@@ -437,14 +480,16 @@ void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
QString
MissionManager
::
_ackTypeToString
(
AckType_t
ackType
)
{
switch
(
ackType
)
{
case
AckNone
:
// State machine is idle
case
AckNone
:
return
QString
(
"No Ack"
);
case
AckMissionCount
:
// MISSION_COUNT message expected
case
AckMissionCount
:
return
QString
(
"MISSION_COUNT"
);
case
AckMissionItem
:
///< MISSION_ITEM expected
case
AckMissionItem
:
return
QString
(
"MISSION_ITEM"
);
case
AckMissionRequest
:
///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
case
AckMissionRequest
:
return
QString
(
"MISSION_REQUEST"
);
case
AckGuidedItem
:
return
QString
(
"Guided Mode Item"
);
default:
qWarning
(
MissionManagerLog
)
<<
"Fell off end of switch statement"
;
return
QString
(
"QGC Internal Error"
);
...
...
src/MissionManager/MissionManager.h
View file @
367b0b85
...
...
@@ -58,6 +58,11 @@ public:
/// @param missionItems Items to send to vehicle
void
writeMissionItems
(
const
QList
<
MissionItem
*>&
missionItems
);
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
/// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
void
writeArduPilotGuidedMissionItem
(
const
QGeoCoordinate
&
gotoCoord
,
bool
altChangeOnly
);
/// Error codes returned in error signal
typedef
enum
{
InternalError
,
...
...
@@ -90,6 +95,7 @@ private:
AckMissionCount
,
///< MISSION_COUNT message expected
AckMissionItem
,
///< MISSION_ITEM expected
AckMissionRequest
,
///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
AckGuidedItem
,
///< MISSION_ACK expected in reponse to ArduPilot guided mode single item send
}
AckType_t
;
void
_startAckTimeout
(
AckType_t
ack
);
...
...
src/Vehicle/Vehicle.cc
View file @
367b0b85
...
...
@@ -99,6 +99,7 @@ Vehicle::Vehicle(LinkInterface* link,
,
_rcRSSI
(
0
)
,
_rcRSSIstore
(
100.0
)
,
_autoDisconnect
(
false
)
,
_flying
(
false
)
,
_connectionLost
(
false
)
,
_connectionLostEnabled
(
true
)
,
_missionManager
(
NULL
)
...
...
@@ -140,7 +141,7 @@ Vehicle::Vehicle(LinkInterface* link,
connect
(
this
,
&
Vehicle
::
_sendMessageOnThread
,
this
,
&
Vehicle
::
_sendMessage
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
_sendMessageOnLinkOnThread
,
this
,
&
Vehicle
::
_sendMessageOnLink
,
Qt
::
QueuedConnection
);
connect
(
this
,
&
Vehicle
::
flightModeChanged
,
this
,
&
Vehicle
::
_
announcef
lightModeChanged
);
connect
(
this
,
&
Vehicle
::
flightModeChanged
,
this
,
&
Vehicle
::
_
handleF
lightModeChanged
);
connect
(
this
,
&
Vehicle
::
armedChanged
,
this
,
&
Vehicle
::
_announceArmedChanged
);
_uas
=
new
UAS
(
_mavlink
,
this
,
_firmwarePluginManager
);
...
...
@@ -376,7 +377,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
}
// Give the plugin a change to adjust the message contents
_firmwarePlugin
->
adjustMavlinkMessage
(
this
,
&
message
);
_firmwarePlugin
->
adjust
Incoming
MavlinkMessage
(
this
,
&
message
);
switch
(
message
.
msgid
)
{
case
MAVLINK_MSG_ID_HOME_POSITION
:
...
...
@@ -412,6 +413,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case
MAVLINK_MSG_ID_VIBRATION
:
_handleVibration
(
message
);
break
;
case
MAVLINK_MSG_ID_EXTENDED_SYS_STATE
:
_handleExtendedSysState
(
message
);
break
;
// Following are ArduPilot dialect messages
...
...
@@ -425,6 +429,23 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas
->
receiveMessage
(
message
);
}
void
Vehicle
::
_handleExtendedSysState
(
mavlink_message_t
&
message
)
{
mavlink_extended_sys_state_t
extendedState
;
mavlink_msg_extended_sys_state_decode
(
&
message
,
&