zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #8350 from DonLakeFlyer/APMFollow 

ArduPilot: Change follow to work above home altitude only
QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
a9493c2d2a de4a15281b
commit
4bf97b5e72
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 109 additions and 41 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
  2. 116
      src/AutoPilotPlugins/APM/APMFollowComponent.qml
  3. 13
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  4. 19
      src/FollowMe/FollowMe.cc

2
src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json
Unescape View File

@ -20,7 +20,7 @@ @@ -20,7 +20,7 @@
},
{
"name": "height",
"shortDescription": "Vertical distance from ground station to vehicle",
"shortDescription": "Vertical distance from Launch (home) position to vehicle",
"type": "double",
"min": 0,
"decimalPlaces": 1,

116
src/AutoPilotPlugins/APM/APMFollowComponent.qml
Unescape View File

@ -49,20 +49,37 @@ SetupPage { @@ -49,20 +49,37 @@ SetupPage {
property int _followComboMaintainIndex: 0
property int _followComboSpecifyIndex: 1
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 _showMainSetup: _followEnabled.rawValue == 1 && _supportedSetup
property bool _showOffsetsSetup: _showMainSetup && !_followMaintain
readonly property int _followYawBehaviorNone: 0
readonly property int _followYawBehaviorFace: 1
readonly property int _followYawBehaviorSame: 2
readonly property int _followYawBehaviorFlight: 3
readonly property int _followOffsetTypeHeadingRelative: 1
readonly property int _followAltitudeTypeAbsolute: 0
readonly property int _followAltitudeTypeRelative: 1
readonly property int _followOffsetTypeRelative: 1
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() {
if (!_followParamsAvailable) {
if (!_followParamsAvailable || !validateSupportedParamSetup()) {
return
}
@ -83,15 +100,24 @@ SetupPage { @@ -83,15 +100,24 @@ SetupPage {
}
controller.height.rawValue = -_followOffsetZ.rawValue
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() {
_followOffsetType.rawValue = 1 // Relative to vehicle
_followSysId.rawValue = 0 // Follow first message sent
_followSysId.rawValue = QGroundControl.mavlinkSystemID
_followOffsetType.rawValue = _followOffsetTypeRelative
if (!_roverFirmware) {
_followAltitudeType.rawValue = _followAltitudeTypeRelative
_followYawBehavior.rawValue = _followYawBehaviorFace
}
controller.distance.value = controller.distance.defaultValue
@ -153,12 +179,11 @@ SetupPage { @@ -153,12 +179,11 @@ SetupPage {
if (!_roverFirmware) {
_followAltitudeType = controller.getParameterFact(-1, "FOLL_ALT_TYPE")
_followYawBehavior = controller.getParameterFact(-1, "FOLL_YAW_BEHAVE")
_followYawBehavior.rawValue = 1 // Point at GCS
}
_followParamsAvailable = true
vehicleParamRefreshLabel.visible = false
_setFollowMeParamDefaults()
validateSupportedParamSetup()
}
}
@ -166,14 +191,13 @@ SetupPage { @@ -166,14 +191,13 @@ SetupPage {
QGCCheckBox {
text: qsTr("Enable Follow Me")
checked: _isFollowMeSetup
checked: _followEnabled.rawValue == 1
onClicked: {
if (checked) {
_followEnabled.rawValue = 1
var missingParameters = [ "FOLL_DIST_MAX", "FOLL_SYSID", "FOLL_OFS_X", "FOLL_OFS_Y", "FOLL_OFS_Z", "FOLL_OFS_TYPE" ]
if (!_roverFirmware) {
missingParameters.push("FOLL_ALT_TYPE")
missingParameters.push("FOLL_YAW_BEHAVE")
missingParameters.push("FOLL_ALT_TYPE", "FOLL_YAW_BEHAVE")
}
controller.getMissingParameters(missingParameters)
vehicleParamRefreshLabel.visible = true
@ -189,15 +213,33 @@ SetupPage { @@ -189,15 +213,33 @@ SetupPage {
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 {
Layout.fillWidth: true
spacing: ScreenTools.defaultFontPixelWidth
visible: _isFollowMeSetup
visible: _showMainSetup
ColumnLayout {
Layout.fillWidth: true
spacing: ScreenTools.defaultFontPixelWidth
visible: _followParamsAvailable && _isFollowMeSetup
GridLayout {
Layout.fillWidth: true
@ -226,10 +268,12 @@ SetupPage { @@ -226,10 +268,12 @@ SetupPage {
QGCComboBox {
id: pointVehicleCombo
Layout.fillWidth: true
// NOTE: The indices for the model below must match the values for FOLL_YAW_BEHAVE
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") ]
model: rgText
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 {
spacing: ScreenTools.defaultFontPixelWidth * 2
visible: _isFollowMeSetup && !_followMaintain
id: offsetSetupLayout
spacing: ScreenTools.defaultFontPixelWidth * 2
visible: _showOffsetsSetup
Item {
height: ScreenTools.defaultFontPixelWidth * 50
@ -392,6 +437,7 @@ SetupPage { @@ -392,6 +437,7 @@ SetupPage {
var x = mouse.x - (width / 2)
var y = (height - mouse.y) - (height / 2)
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 {
id: gcsIconHeight
source: "/res/QGCLogoArrow"
mipmap: true
antialiasing: true
fillMode: Image.PreserveAspectFit
height: ScreenTools.defaultFontPixelHeight * 2.5
sourceSize.height: height
transform: Rotation {
origin.x: gcsIconHeight.width / 2
origin.y: gcsIconHeight.height / 2
angle: 65
axis { x: 1; y: 0; z: 0 }
}
MissionItemIndexLabel {
id: launchIconHeight
Layout.alignment: Qt.AlignHCenter
label: qsTr("L")
transform: [
Scale {
origin.x: launchIconHeight.width / 2
origin.y: launchIconHeight.height / 2
xScale: 1.5
yScale: 2.5
},
Rotation {
origin.x: launchIconHeight.width / 2
origin.y: launchIconHeight.height / 2
angle: 75
axis { x: 1; y: 0; z: 0 }
} ]
}
}
}

13
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
Unescape View File

@ -1136,15 +1136,16 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti @@ -1136,15 +1136,16 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
mavlink_global_position_int_t 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.lat = motionReport.lat_int;
globalPositionInt.lon = motionReport.lon_int;
globalPositionInt.alt = static_cast<int32_t>(motionReport.altMetersAMSL * 1000); // mm
globalPositionInt.relative_alt = static_cast<int32_t>((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm
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.vzMetersPerSec * 100); // cm/sec
globalPositionInt.hdg = static_cast<uint16_t>(motionReport.headingDegrees * 100.0); // centi-degrees
globalPositionInt.alt = static_cast<int32_t>(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.vy = static_cast<int16_t>(motionReport.vyMetersPerSec * 100); // cm/sec
globalPositionInt.vy = static_cast<int16_t>(motionReport.vzMetersPerSec * 100); // cm/sec
globalPositionInt.hdg = static_cast<uint16_t>(motionReport.headingDegrees * 100.0); // centi-degrees
mavlink_message_t message;
mavlink_msg_global_position_int_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),

19
src/FollowMe/FollowMe.cc
Unescape View File

@ -84,6 +84,23 @@ void FollowMe::_sendGCSMotionReport() @@ -84,6 +84,23 @@ void FollowMe::_sendGCSMotionReport()
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 = {};
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++) {
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;
vehicle->firmwarePlugin()->sendGCSMotionReport(vehicle, motionReport, estimatation_capabilities);
}

Write Preview
Loading…
Cancel
Save