Skip to content

Q
qgroundcontrol
Unverified Commit 4bf97b5e authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #8350 from DonLakeFlyer/APMFollow 

ArduPilot: Change follow to work above home altitude only
parents a9493c2d de4a1528
Show whitespace changes
Inline Side-by-side
Showing with 109 additions and 41 deletions
src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
View file @ 4bf97b5e
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
}, },
{ {
"name": "height", "name": "height",
"shortDescription": "Vertical distance from ground station to vehicle", "shortDescription": "Vertical distance from Launch (home) position to vehicle",
"type": "double", "type": "double",
"min": 0, "min": 0,
"decimalPlaces": 1, "decimalPlaces": 1,
......
src/AutoPilotPlugins/APM/APMFollowComponent.qml
View file @ 4bf97b5e
...@@ -49,20 +49,37 @@ SetupPage { ...@@ -49,20 +49,37 @@ SetupPage {
property int _followComboMaintainIndex: 0 property int _followComboMaintainIndex: 0
property int _followComboSpecifyIndex: 1 property int _followComboSpecifyIndex: 1
property bool _followMaintain: followPositionCombo.currentIndex === _followComboMaintainIndex property bool _followMaintain: followPositionCombo.currentIndex === _followComboMaintainIndex
property bool _isFollowMeSetup: _followEnabled.rawValue == 1 && _followParamsAvailable && _followOffsetType.rawValue == _followOffsetTypeHeadingRelative && _followSysId.rawValue == 0 property bool _supportedSetup: true
property bool _roverFirmware: controller.roverFirmware property bool _roverFirmware: controller.roverFirmware
property bool _showMainSetup: _followEnabled.rawValue == 1 && _supportedSetup
property bool _showOffsetsSetup: _showMainSetup && !_followMaintain
readonly property int _followYawBehaviorNone: 0 readonly property int _followYawBehaviorNone: 0
readonly property int _followYawBehaviorFace: 1 readonly property int _followYawBehaviorFace: 1
readonly property int _followYawBehaviorSame: 2 readonly property int _followYawBehaviorSame: 2
readonly property int _followYawBehaviorFlight: 3 readonly property int _followYawBehaviorFlight: 3
readonly property int _followOffsetTypeHeadingRelative: 1 readonly property int _followAltitudeTypeAbsolute: 0
readonly property int _followAltitudeTypeRelative: 1 readonly property int _followAltitudeTypeRelative: 1
readonly property int _followOffsetTypeRelative: 1
Component.onCompleted: _setUIFromParams() Component.onCompleted: _setUIFromParams()
function validateSupportedParamSetup() {
var followSysIdOk = _followSysId.rawValue == QGroundControl.mavlinkSystemID
var followOffsetOk = _followOffsetType.rawValue == _followOffsetTypeRelative
var followAltOk = true
var followYawOk = true
if (!_roverFirmware) {
followAltOk = _followAltitudeType.rawValue == _followAltitudeTypeRelative
followYawOk = _followYawBehavior.rawValue == _followYawBehaviorNone || _followYawBehavior.rawValue == _followYawBehaviorFace || _followYawBehavior.rawValue == _followYawBehaviorFlight
}
_supportedSetup = followOffsetOk && followAltOk && followYawOk && followSysIdOk
console.log("_supportedSetup", _supportedSetup, followSysIdOk, followOffsetOk, followAltOk, followYawOk)
return _supportedSetup
}
function _setUIFromParams() { function _setUIFromParams() {
if (!_followParamsAvailable) { if (!_followParamsAvailable || !validateSupportedParamSetup()) {
return return
} }
...@@ -83,15 +100,24 @@ SetupPage { ...@@ -83,15 +100,24 @@ SetupPage {
} }
controller.height.rawValue = -_followOffsetZ.rawValue controller.height.rawValue = -_followOffsetZ.rawValue
if (!_roverFirmware) { if (!_roverFirmware) {
pointVehicleCombo.currentIndex = _followYawBehavior.rawValue var comboIndex = -1
for (var i=0; i<pointVehicleCombo.rgValues.length; i++) {
if (pointVehicleCombo.rgValues[i] == _followYawBehavior.rawValue) {
comboIndex = i
break
}
}
pointVehicleCombo.currentIndex = comboIndex
} }
} }
function _setFollowMeParamDefaults() { function _setFollowMeParamDefaults() {
_followOffsetType.rawValue = 1 // Relative to vehicle _followSysId.rawValue = QGroundControl.mavlinkSystemID
_followSysId.rawValue = 0 // Follow first message sent _followOffsetType.rawValue = _followOffsetTypeRelative
if (!_roverFirmware) { if (!_roverFirmware) {
_followAltitudeType.rawValue = _followAltitudeTypeRelative _followAltitudeType.rawValue = _followAltitudeTypeRelative
_followYawBehavior.rawValue = _followYawBehaviorFace
} }
controller.distance.value = controller.distance.defaultValue controller.distance.value = controller.distance.defaultValue
...@@ -153,12 +179,11 @@ SetupPage { ...@@ -153,12 +179,11 @@ SetupPage {
if (!_roverFirmware) { if (!_roverFirmware) {
_followAltitudeType = controller.getParameterFact(-1, "FOLL_ALT_TYPE") _followAltitudeType = controller.getParameterFact(-1, "FOLL_ALT_TYPE")
_followYawBehavior = controller.getParameterFact(-1, "FOLL_YAW_BEHAVE") _followYawBehavior = controller.getParameterFact(-1, "FOLL_YAW_BEHAVE")
_followYawBehavior.rawValue = 1 // Point at GCS
} }
_followParamsAvailable = true _followParamsAvailable = true
vehicleParamRefreshLabel.visible = false vehicleParamRefreshLabel.visible = false
_setFollowMeParamDefaults() validateSupportedParamSetup()
} }
} }
...@@ -166,14 +191,13 @@ SetupPage { ...@@ -166,14 +191,13 @@ SetupPage {
QGCCheckBox { QGCCheckBox {
text: qsTr("Enable Follow Me") text: qsTr("Enable Follow Me")
checked: _isFollowMeSetup checked: _followEnabled.rawValue == 1
onClicked: { onClicked: {
if (checked) { if (checked) {
_followEnabled.rawValue = 1 _followEnabled.rawValue = 1
var missingParameters = [ "FOLL_DIST_MAX", "FOLL_SYSID", "FOLL_OFS_X", "FOLL_OFS_Y", "FOLL_OFS_Z", "FOLL_OFS_TYPE" ] var missingParameters = [ "FOLL_DIST_MAX", "FOLL_SYSID", "FOLL_OFS_X", "FOLL_OFS_Y", "FOLL_OFS_Z", "FOLL_OFS_TYPE" ]
if (!_roverFirmware) { if (!_roverFirmware) {
missingParameters.push("FOLL_ALT_TYPE") missingParameters.push("FOLL_ALT_TYPE", "FOLL_YAW_BEHAVE")
missingParameters.push("FOLL_YAW_BEHAVE")
} }
controller.getMissingParameters(missingParameters) controller.getMissingParameters(missingParameters)
vehicleParamRefreshLabel.visible = true vehicleParamRefreshLabel.visible = true
...@@ -189,15 +213,33 @@ SetupPage { ...@@ -189,15 +213,33 @@ SetupPage {
visible: false visible: false
} }
Column {
width: offsetSetupLayout.width
spacing: ScreenTools.defaultFontPixelWidth
visible: !_supportedSetup
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("The vehicle parameters required for follow me are currently set in a way which is not supported. Using follow with this setup may lead to unpredictable/hazardous results.")
wrapMode: Text.WordWrap
onWidthChanged: console.log('width', width)
}
QGCButton {
text: qsTr("Reset To Supported Settings")
onClicked: _setFollowMeParamDefaults()
}
}
ColumnLayout { ColumnLayout {
Layout.fillWidth: true Layout.fillWidth: true
spacing: ScreenTools.defaultFontPixelWidth spacing: ScreenTools.defaultFontPixelWidth
visible: _isFollowMeSetup visible: _showMainSetup
ColumnLayout { ColumnLayout {
Layout.fillWidth: true Layout.fillWidth: true
spacing: ScreenTools.defaultFontPixelWidth spacing: ScreenTools.defaultFontPixelWidth
visible: _followParamsAvailable && _isFollowMeSetup
GridLayout { GridLayout {
Layout.fillWidth: true Layout.fillWidth: true
...@@ -226,10 +268,12 @@ SetupPage { ...@@ -226,10 +268,12 @@ SetupPage {
QGCComboBox { QGCComboBox {
id: pointVehicleCombo id: pointVehicleCombo
Layout.fillWidth: true Layout.fillWidth: true
// NOTE: The indices for the model below must match the values for FOLL_YAW_BEHAVE model: rgText
model: [ qsTr("Maintain current vehicle orientation"), qsTr("Point at ground station location"), qsTr("Same orientation as ground station"), qsTr("Same direction as ground station movement") ]
visible: !_roverFirmware visible: !_roverFirmware
onActivated: _followYawBehavior.rawValue = index onActivated: _followYawBehavior.rawValue = rgValues[index]
property var rgText: [ qsTr("Maintain current vehicle orientation"), qsTr("Point at ground station location"), qsTr("Same direction as ground station movement") ]
property var rgValues: [ _followYawBehaviorNone, _followYawBehaviorFace, _followYawBehaviorFlight ]
} }
} }
...@@ -271,8 +315,9 @@ SetupPage { ...@@ -271,8 +315,9 @@ SetupPage {
} }
RowLayout { RowLayout {
id: offsetSetupLayout
spacing: ScreenTools.defaultFontPixelWidth * 2 spacing: ScreenTools.defaultFontPixelWidth * 2
visible: _isFollowMeSetup && !_followMaintain visible: _showOffsetsSetup
Item { Item {
height: ScreenTools.defaultFontPixelWidth * 50 height: ScreenTools.defaultFontPixelWidth * 50
...@@ -392,6 +437,7 @@ SetupPage { ...@@ -392,6 +437,7 @@ SetupPage {
var x = mouse.x - (width / 2) var x = mouse.x - (width / 2)
var y = (height - mouse.y) - (height / 2) var y = (height - mouse.y) - (height / 2)
controller.angle.rawValue = _radiansToHeading(Math.atan2(y, x)) controller.angle.rawValue = _radiansToHeading(Math.atan2(y, x))
_setXYOffsetByAngleAndDistance(controller.angle.rawValue, controller.distance.rawValue)
} }
} }
} }
...@@ -455,21 +501,25 @@ SetupPage { ...@@ -455,21 +501,25 @@ SetupPage {
} }
} }
Image { MissionItemIndexLabel {
id: gcsIconHeight id: launchIconHeight
source: "/res/QGCLogoArrow" Layout.alignment: Qt.AlignHCenter
mipmap: true label: qsTr("L")
antialiasing: true
fillMode: Image.PreserveAspectFit transform: [
height: ScreenTools.defaultFontPixelHeight * 2.5 Scale {
sourceSize.height: height origin.x: launchIconHeight.width / 2
origin.y: launchIconHeight.height / 2
transform: Rotation { xScale: 1.5
origin.x: gcsIconHeight.width / 2 yScale: 2.5
origin.y: gcsIconHeight.height / 2
angle: 65 },
Rotation {
origin.x: launchIconHeight.width / 2
origin.y: launchIconHeight.height / 2
angle: 75
axis { x: 1; y: 0; z: 0 } axis { x: 1; y: 0; z: 0 }
} } ]
} }
} }
} }
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @ 4bf97b5e
...@@ -1136,11 +1136,12 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti ...@@ -1136,11 +1136,12 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
mavlink_global_position_int_t globalPositionInt; mavlink_global_position_int_t globalPositionInt;
memset(&globalPositionInt, 0, sizeof(globalPositionInt)); memset(&globalPositionInt, 0, sizeof(globalPositionInt));
// Important note: QGC only supports sending the constant GCS home position altitude for follow me.
globalPositionInt.time_boot_ms = static_cast<uint32_t>(qgcApp()->msecsSinceBoot()); globalPositionInt.time_boot_ms = static_cast<uint32_t>(qgcApp()->msecsSinceBoot());
globalPositionInt.lat = motionReport.lat_int; globalPositionInt.lat = motionReport.lat_int;
globalPositionInt.lon = motionReport.lon_int; globalPositionInt.lon = motionReport.lon_int;
globalPositionInt.alt = static_cast<int32_t>(motionReport.altMetersAMSL * 1000); // mm globalPositionInt.alt = static_cast<int32_t>(vehicle->homePosition().altitude() * 1000); // mm
globalPositionInt.relative_alt = static_cast<int32_t>((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm globalPositionInt.relative_alt = static_cast<int32_t>(0); // mm
globalPositionInt.vx = static_cast<int16_t>(motionReport.vxMetersPerSec * 100); // cm/sec globalPositionInt.vx = static_cast<int16_t>(motionReport.vxMetersPerSec * 100); // cm/sec
globalPositionInt.vy = static_cast<int16_t>(motionReport.vyMetersPerSec * 100); // cm/sec globalPositionInt.vy = static_cast<int16_t>(motionReport.vyMetersPerSec * 100); // cm/sec
globalPositionInt.vy = static_cast<int16_t>(motionReport.vzMetersPerSec * 100); // cm/sec globalPositionInt.vy = static_cast<int16_t>(motionReport.vzMetersPerSec * 100); // cm/sec
......
src/FollowMe/FollowMe.cc
View file @ 4bf97b5e
...@@ -84,6 +84,23 @@ void FollowMe::_sendGCSMotionReport() ...@@ -84,6 +84,23 @@ void FollowMe::_sendGCSMotionReport()
return; return;
} }
// First check to see if any vehicles need follow me updates
bool needFollowMe = false;
if (_currentMode == MODE_ALWAYS) {
needFollowMe = true;
} else if (_currentMode == MODE_FOLLOWME) {
QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles();
for (int i=0; i<vehicles->count(); i++) {
Vehicle* vehicle = vehicles->value<Vehicle*>(i);
if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
needFollowMe = true;
}
}
}
if (!needFollowMe) {
return;
}
GCSMotionReport motionReport = {}; GCSMotionReport motionReport = {};
uint8_t estimatation_capabilities = 0; uint8_t estimatation_capabilities = 0;
...@@ -134,7 +151,7 @@ void FollowMe::_sendGCSMotionReport() ...@@ -134,7 +151,7 @@ void FollowMe::_sendGCSMotionReport()
for (int i=0; i<vehicles->count(); i++) { for (int i=0; i<vehicles->count(); i++) {
Vehicle* vehicle = vehicles->value<Vehicle*>(i); Vehicle* vehicle = vehicles->value<Vehicle*>(i);
if (_currentMode == MODE_ALWAYS || (_currentMode == MODE_FOLLOWME && _isFollowFlightMode(vehicle, vehicle->flightMode()))) { if (_isFollowFlightMode(vehicle, vehicle->flightMode())) {
qCDebug(FollowMeLog) << "sendGCSMotionReport latInt:lonInt:altMetersAMSL" << motionReport.lat_int << motionReport.lon_int << motionReport.altMetersAMSL; qCDebug(FollowMeLog) << "sendGCSMotionReport latInt:lonInt:altMetersAMSL" << motionReport.lat_int << motionReport.lon_int << motionReport.altMetersAMSL;
vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities); vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment