Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Asif Khan 2023-05-22 09:32:38 +05:30 committed by Tom Pittenger
parent fdac668782
commit d367483155
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Follow/AP_Follow.cpp
View File

@ -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), AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
#endif #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 AP_GROUPEND
}; };

12
libraries/AP_Follow/AP_Follow.h
View File

@ -28,6 +28,11 @@ class AP_Follow
public: public:
// enum for FOLLOW_OPTIONS parameter
enum class Option {
MOUNT_FOLLOW_ON_ENTER = 1
};
// enum for YAW_BEHAVE parameter // enum for YAW_BEHAVE parameter
enum YawBehave { enum YawBehave {
YAW_BEHAVE_NONE = 0, 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 // 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); 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 // 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; } const AC_P& get_pos_p() const { return _p_pos; }
@ -98,6 +106,9 @@ public:
// get system time of last position update // get system time of last position update
uint32_t get_last_update_ms() const { return _last_location_update_ms; } 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 // parameter list
static const struct AP_Param::GroupInfo var_info[]; 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 _yaw_behave; // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
AP_Int8 _alt_type; // altitude source for follow mode AP_Int8 _alt_type; // altitude source for follow mode
AC_P _p_pos; // position error P controller AC_P _p_pos; // position error P controller
AP_Int16 _options; // options for mount behaviour follow mode
// local variables // local variables
bool _healthy; // true if we are receiving mavlink messages (regardless of whether they have target position info within them) bool _healthy; // true if we are receiving mavlink messages (regardless of whether they have target position info within them)