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
724df478
Commit
724df478
authored
Sep 06, 2017
by
DonLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Centralized guided support for all ArduPilot vehicles
parent
6ef825e8
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
96 additions
and
98 deletions
+96
-98
APMFirmwarePlugin.cc
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+73
-12
APMFirmwarePlugin.h
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+8
-2
ArduCopterFirmwarePlugin.cc
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+0
-72
ArduCopterFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+0
-8
ArduPlaneFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h
+4
-3
ArduRoverFirmwarePlugin.cc
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
+8
-0
ArduRoverFirmwarePlugin.h
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
+3
-1
No files found.
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @
724df478
...
...
@@ -7,10 +7,6 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "APMFirmwarePlugin.h"
#include "APMAutoPilotPlugin.h"
#include "QGCMAVLink.h"
...
...
@@ -18,6 +14,7 @@
#include "APMFlightModesComponentController.h"
#include "APMAirframeComponentController.h"
#include "APMSensorsComponentController.h"
#include "MissionManager.h"
#include <QTcpSocket>
...
...
@@ -159,9 +156,13 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
return
new
APMAutoPilotPlugin
(
vehicle
,
vehicle
);
}
bool
APMFirmwarePlugin
::
isCapable
(
const
Vehicle
*
/*vehicle*/
,
FirmwareCapabilities
capabilities
)
bool
APMFirmwarePlugin
::
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
{
return
(
capabilities
&
(
SetFlightModeCapability
|
PauseVehicleCapability
))
==
capabilities
;
Q_UNUSED
(
vehicle
);
uint32_t
vehicleCapabilities
=
SetFlightModeCapability
|
GuidedModeCapability
|
PauseVehicleCapability
;
return
(
capabilities
&
vehicleCapabilities
)
==
capabilities
;
}
QList
<
VehicleComponent
*>
APMFirmwarePlugin
::
componentsForVehicle
(
AutoPilotPlugin
*
vehicle
)
...
...
@@ -725,12 +726,6 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
return
vehicle
->
flightMode
()
==
"Guided"
;
}
void
APMFirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
// Best we can do in this case
vehicle
->
setFlightMode
(
"Loiter"
);
}
void
APMFirmwarePlugin
::
_soloVideoHandshake
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
...
...
@@ -819,3 +814,69 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
return
QString
();
}
}
void
APMFirmwarePlugin
::
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
{
if
(
guidedMode
)
{
_setFlightModeAndValidate
(
vehicle
,
"Guided"
);
}
else
{
pauseVehicle
(
vehicle
);
}
}
void
APMFirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
_setFlightModeAndValidate
(
vehicle
,
pauseFlightMode
());
}
void
APMFirmwarePlugin
::
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
;
}
setGuidedMode
(
vehicle
,
true
);
QGeoCoordinate
coordWithAltitude
=
gotoCoord
;
coordWithAltitude
.
setAltitude
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
());
vehicle
->
missionManager
()
->
writeArduPilotGuidedMissionItem
(
coordWithAltitude
,
false
/* altChangeOnly */
);
}
void
APMFirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
{
_setFlightModeAndValidate
(
vehicle
,
rtlFlightMode
());
}
void
APMFirmwarePlugin
::
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
{
if
(
qIsNaN
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
()))
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Unable to change altitude, vehicle altitude not known."
));
return
;
}
setGuidedMode
(
vehicle
,
true
);
mavlink_message_t
msg
;
mavlink_set_position_target_local_ned_t
cmd
;
memset
(
&
cmd
,
0
,
sizeof
(
cmd
));
cmd
.
target_system
=
vehicle
->
id
();
cmd
.
target_component
=
vehicle
->
defaultComponentId
();
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
=
-
(
altitudeChange
);
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
mavlink_msg_set_position_target_local_ned_encode_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
&
cmd
);
vehicle
->
sendMessageOnLink
(
vehicle
->
priorityLink
(),
msg
);
}
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @
724df478
...
...
@@ -74,12 +74,18 @@ public:
QList
<
MAV_CMD
>
supportedMissionCommands
(
void
)
final
;
AutoPilotPlugin
*
autopilotPlugin
(
Vehicle
*
vehicle
)
final
;
bool
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
override
;
bool
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
final
;
void
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
final
;
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
final
;
QStringList
flightModes
(
Vehicle
*
vehicle
)
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
)
override
;
QString
rtlFlightMode
(
void
)
const
final
{
return
QString
(
"RTL"
);
}
QString
missionFlightMode
(
void
)
const
final
{
return
QString
(
"Auto"
);
}
void
pauseVehicle
(
Vehicle
*
vehicle
)
final
;
void
guidedModeRTL
(
Vehicle
*
vehicle
)
final
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
override
;
int
manualControlReservedButtonCount
(
void
)
override
;
bool
adjustIncomingMavlinkMessage
(
Vehicle
*
vehicle
,
mavlink_message_t
*
message
)
override
;
void
adjustOutgoingMavlinkMessage
(
Vehicle
*
vehicle
,
LinkInterface
*
outgoingLink
,
mavlink_message_t
*
message
)
final
;
...
...
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @
724df478
...
...
@@ -151,20 +151,6 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
return
majorVersionNumber
==
3
?
5
:
Vehicle
::
versionNotSetValue
;
}
bool
ArduCopterFirmwarePlugin
::
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
{
Q_UNUSED
(
vehicle
);
uint32_t
vehicleCapabilities
=
SetFlightModeCapability
|
GuidedModeCapability
|
PauseVehicleCapability
;
return
(
capabilities
&
vehicleCapabilities
)
==
capabilities
;
}
void
ArduCopterFirmwarePlugin
::
guidedModeRTL
(
Vehicle
*
vehicle
)
{
_setFlightModeAndValidate
(
vehicle
,
"RTL"
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeLand
(
Vehicle
*
vehicle
)
{
_setFlightModeAndValidate
(
vehicle
,
"Land"
);
...
...
@@ -209,64 +195,6 @@ bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
return
true
;
}
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
;
}
QGeoCoordinate
coordWithAltitude
=
gotoCoord
;
coordWithAltitude
.
setAltitude
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
());
vehicle
->
missionManager
()
->
writeArduPilotGuidedMissionItem
(
coordWithAltitude
,
false
/* altChangeOnly */
);
}
void
ArduCopterFirmwarePlugin
::
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
{
if
(
qIsNaN
(
vehicle
->
altitudeRelative
()
->
rawValue
().
toDouble
()))
{
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Unable to change altitude, vehicle altitude not known."
));
return
;
}
setGuidedMode
(
vehicle
,
true
);
mavlink_message_t
msg
;
mavlink_set_position_target_local_ned_t
cmd
;
memset
(
&
cmd
,
0
,
sizeof
(
cmd
));
cmd
.
target_system
=
vehicle
->
id
();
cmd
.
target_component
=
vehicle
->
defaultComponentId
();
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
=
-
(
altitudeChange
);
MAVLinkProtocol
*
mavlink
=
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
();
mavlink_msg_set_position_target_local_ned_encode_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
&
cmd
);
vehicle
->
sendMessageOnLink
(
vehicle
->
priorityLink
(),
msg
);
}
void
ArduCopterFirmwarePlugin
::
pauseVehicle
(
Vehicle
*
vehicle
)
{
_setFlightModeAndValidate
(
vehicle
,
"Brake"
);
}
void
ArduCopterFirmwarePlugin
::
setGuidedMode
(
Vehicle
*
vehicle
,
bool
guidedMode
)
{
if
(
guidedMode
)
{
_setFlightModeAndValidate
(
vehicle
,
"Guided"
);
}
else
{
pauseVehicle
(
vehicle
);
}
}
bool
ArduCopterFirmwarePlugin
::
multiRotorCoaxialMotors
(
Vehicle
*
vehicle
)
{
Q_UNUSED
(
vehicle
);
...
...
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @
724df478
...
...
@@ -55,22 +55,14 @@ public:
ArduCopterFirmwarePlugin
(
void
);
// Overrides from FirmwarePlugin
bool
isCapable
(
const
Vehicle
*
vehicle
,
FirmwareCapabilities
capabilities
)
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
)
final
;
void
guidedModeGotoLocation
(
Vehicle
*
vehicle
,
const
QGeoCoordinate
&
gotoCoord
)
final
;
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
final
;
const
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
&
paramNameRemapMajorVersionMap
(
void
)
const
final
{
return
_remapParamName
;
}
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
bool
multiRotorCoaxialMotors
(
Vehicle
*
vehicle
)
final
;
bool
multiRotorXConfig
(
Vehicle
*
vehicle
)
final
;
QString
offlineEditingParamFile
(
Vehicle
*
vehicle
)
final
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
":/FirmwarePlugin/APM/Copter.OfflineEditing.params"
);
}
QString
pauseFlightMode
(
void
)
const
override
{
return
QString
(
"Brake"
);
}
QString
missionFlightMode
(
void
)
const
override
{
return
QString
(
"Auto"
);
}
QString
rtlFlightMode
(
void
)
const
override
{
return
QString
(
"RTL"
);
}
QString
landFlightMode
(
void
)
const
override
{
return
QString
(
"Land"
);
}
QString
takeControlFlightMode
(
void
)
const
override
{
return
QString
(
"Loiter"
);
}
bool
vehicleYawsToNextWaypointInMission
(
const
Vehicle
*
vehicle
)
const
final
;
...
...
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h
View file @
724df478
...
...
@@ -56,10 +56,11 @@ public:
ArduPlaneFirmwarePlugin
(
void
);
// Overrides from FirmwarePlugin
QString
offlineEditingParamFile
(
Vehicle
*
vehicle
)
final
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
":/FirmwarePlugin/APM/Plane.OfflineEditing.params"
);
}
QString
pauseFlightMode
(
void
)
const
override
{
return
QString
(
"Loiter"
);
}
QString
offlineEditingParamFile
(
Vehicle
*
vehicle
)
final
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
":/FirmwarePlugin/APM/Plane.OfflineEditing.params"
);
}
QString
autoDisarmParameter
(
Vehicle
*
vehicle
)
final
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
"LAND_DISARMDELAY"
);
}
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
const
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
&
paramNameRemapMajorVersionMap
(
void
)
const
final
{
return
_remapParamName
;
}
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
QString
autoDisarmParameter
(
Vehicle
*
vehicle
)
final
{
Q_UNUSED
(
vehicle
);
return
QStringLiteral
(
"LAND_DISARMDELAY"
);
}
private:
static
bool
_remapParamNameIntialized
;
...
...
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
View file @
724df478
...
...
@@ -8,6 +8,7 @@
****************************************************************************/
#include "ArduRoverFirmwarePlugin.h"
#include "QGCApplication.h"
bool
ArduRoverFirmwarePlugin
::
_remapParamNameIntialized
=
false
;
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
ArduRoverFirmwarePlugin
::
_remapParamName
;
...
...
@@ -98,3 +99,10 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer
return
majorVersionNumber
==
3
?
2
:
Vehicle
::
versionNotSetValue
;
}
void
ArduRoverFirmwarePlugin
::
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
{
Q_UNUSED
(
vehicle
);
Q_UNUSED
(
altitudeChange
);
qgcApp
()
->
showMessage
(
QStringLiteral
(
"Change altitude not supported."
));
}
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
View file @
724df478
...
...
@@ -51,8 +51,10 @@ public:
ArduRoverFirmwarePlugin
(
void
);
// Overrides from FirmwarePlugin
QString
pauseFlightMode
(
void
)
const
override
{
return
QString
(
"Hold"
);
}
void
guidedModeChangeAltitude
(
Vehicle
*
vehicle
,
double
altitudeChange
)
final
;
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
const
FirmwarePlugin
::
remapParamNameMajorVersionMap_t
&
paramNameRemapMajorVersionMap
(
void
)
const
final
{
return
_remapParamName
;
}
int
remapParamNameHigestMinorVersionNumber
(
int
majorVersionNumber
)
const
final
;
private:
static
bool
_remapParamNameIntialized
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment