Merge pull request #8350 from DonLakeFlyer/APMFollow

ArduPilot: Change follow to work above home altitude only

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,

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -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()),

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; 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()
 
     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);
         }