From de4a15281be2355d28b74620c3cacddc7988a405 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Mon, 17 Feb 2020 12:18:45 -0800 Subject: [PATCH] Change follow to work above home altitude only --- .../APM/APMFollowComponent.FactMetaData.json | 2 +- .../APM/APMFollowComponent.qml | 116 +++++++++++++----- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 13 +- src/FollowMe/FollowMe.cc | 19 ++- 4 files changed, 109 insertions(+), 41 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json b/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json index 297b030d3..7d0269718 100644 --- a/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json +++ b/src/AutoPilotPlugins/APM/APMFollowComponent.FactMetaData.json @@ -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, diff --git a/src/AutoPilotPlugins/APM/APMFollowComponent.qml b/src/AutoPilotPlugins/APM/APMFollowComponent.qml index 8cd35ce0e..77c6c9302 100644 --- a/src/AutoPilotPlugins/APM/APMFollowComponent.qml +++ b/src/AutoPilotPlugins/APM/APMFollowComponent.qml @@ -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 { } controller.height.rawValue = -_followOffsetZ.rawValue if (!_roverFirmware) { - pointVehicleCombo.currentIndex = _followYawBehavior.rawValue + var comboIndex = -1 + for (var i=0; i(qgcApp()->msecsSinceBoot()); globalPositionInt.lat = motionReport.lat_int; globalPositionInt.lon = motionReport.lon_int; - globalPositionInt.alt = static_cast(motionReport.altMetersAMSL * 1000); // mm - globalPositionInt.relative_alt = static_cast((motionReport.altMetersAMSL - vehicle->homePosition().altitude()) * 1000); // mm - globalPositionInt.vx = static_cast(motionReport.vxMetersPerSec * 100); // cm/sec - globalPositionInt.vy = static_cast(motionReport.vyMetersPerSec * 100); // cm/sec - globalPositionInt.vy = static_cast(motionReport.vzMetersPerSec * 100); // cm/sec - globalPositionInt.hdg = static_cast(motionReport.headingDegrees * 100.0); // centi-degrees + globalPositionInt.alt = static_cast(vehicle->homePosition().altitude() * 1000); // mm + globalPositionInt.relative_alt = static_cast(0); // mm + globalPositionInt.vx = static_cast(motionReport.vxMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vyMetersPerSec * 100); // cm/sec + globalPositionInt.vy = static_cast(motionReport.vzMetersPerSec * 100); // cm/sec + globalPositionInt.hdg = static_cast(motionReport.headingDegrees * 100.0); // centi-degrees mavlink_message_t message; mavlink_msg_global_position_int_encode_chan(static_cast(mavlinkProtocol->getSystemId()), diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index 2f35b493d..b75df953a 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -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; icount(); i++) { + Vehicle* vehicle = vehicles->value(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() for (int i=0; icount(); i++) { Vehicle* vehicle = vehicles->value(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); } -- 2.22.0