AP_Follow: factor out a Log_Write_FOLL method

libraries/AP_Follow/AP_Follow.cpp

@@ -411,6 +411,15 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
 
     if (updated) {
 #if HAL_LOGGING_ENABLED
+        Log_Write_FOLL();
+#endif
+    }
+}
+
+// write out an onboard-log message to help diagnose follow problems:
+#if HAL_LOGGING_ENABLED
+void AP_Follow::Log_Write_FOLL()
+{
         // get estimated location and velocity
         Location loc_estimate{};
         Vector3f vel_estimate;
@@ -445,9 +454,8 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
                                                loc_estimate.lng,
                                                loc_estimate.alt
                                                );
-#endif
-    }
 }
+#endif  // HAL_LOGGING_ENABLED
 
 // get velocity estimate in m/s in NED frame using dt since last update
 bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const

libraries/AP_Follow/AP_Follow.h

@@ -136,6 +136,9 @@ private:
     // set recorded distance and bearing to target to zero
     void clear_dist_and_bearing_to_target();
 
+    // write out an onboard-log message to help diagnose follow problems:
+    void Log_Write_FOLL();
+
     // parameters
     AP_Int8     _enabled;           // 1 if this subsystem is enabled
     AP_Int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)