AP_Follow: support for Mount following the lead vehicle in follow mode

ArduCopter/mode_follow.cpp

@@ -20,6 +20,15 @@ bool ModeFollow::init(const bool ignore_checks)
         gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
         return false;
     }
+
+#if HAL_MOUNT_ENABLED
+    AP_Mount *mount = AP_Mount::get_singleton();
+    // follow the lead vehicle using sysid
+    if (g2.follow.option_is_enabled(AP_Follow::Option::MOUNT_FOLLOW_ON_ENTER) && mount != nullptr) {
+        mount->set_target_sysid(g2.follow.get_target_sysid());
+    }
+#endif
+
     // re-use guided mode
     return ModeGuided::init(ignore_checks);
 }

libraries/AP_Follow/AP_Follow.cpp

@@ -130,6 +130,13 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+    // @Param: _OPTIONS
+    // @DisplayName: Follow options
+    // @Description: Follow options bitmask
+    // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
+    // @User: Standard
+    AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
+
     AP_GROUPEND
 };
 

libraries/AP_Follow/AP_Follow.h

@@ -28,6 +28,11 @@ class AP_Follow
 
 public:
 
+    // enum for FOLLOW_OPTIONS parameter
+    enum class Option {
+        MOUNT_FOLLOW_ON_ENTER = 1
+    };
+
     // enum for YAW_BEHAVE parameter
     enum YawBehave {
         YAW_BEHAVE_NONE = 0,
@@ -69,6 +74,9 @@ class AP_Follow
     // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
     bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
 
+    // get target sysid
+    uint8_t get_target_sysid() const { return _sysid.get(); }
+
     // get position controller.  this controller is not used within this library but it is convenient to hold it here
     const AC_P& get_pos_p() const { return _p_pos; }
 
@@ -98,6 +106,9 @@ class AP_Follow
     // get system time of last position update
     uint32_t get_last_update_ms() const { return _last_location_update_ms; }
 
+    // returns true if a follow option enabled
+    bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -128,6 +139,7 @@ class AP_Follow
     AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
     AP_Int8     _alt_type;          // altitude source for follow mode
     AC_P        _p_pos;             // position error P controller
+    AP_Int16    _options;           // options for mount behaviour follow mode
 
     // local variables
     bool _healthy;                  // true if we are receiving mavlink messages (regardless of whether they have target position info within them)