From d36748315523dc9afb88b8bf357fb532491b47ee Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Mon, 22 May 2023 09:32:38 +0530 Subject: [PATCH] AP_Follow: support for Mount following the lead vehicle in follow mode --- libraries/AP_Follow/AP_Follow.cpp | 7 +++++++ libraries/AP_Follow/AP_Follow.h | 12 ++++++++++++ 2 files changed, 19 insertions(+) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 7df9bab818..15196278be 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/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 }; diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 623bb748ca..bc6cf7b639 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/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 @@ public: // 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 @@ public: // 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 @@ private: 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)