Line data Source code
1 : /* 2 : This program is free software: you can redistribute it and/or modify 3 : it under the terms of the GNU General Public License as published by 4 : the Free Software Foundation, either version 3 of the License, or 5 : (at your option) any later version. 6 : 7 : This program is distributed in the hope that it will be useful, 8 : but WITHOUT ANY WARRANTY; without even the implied warranty of 9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 : GNU General Public License for more details. 11 : 12 : You should have received a copy of the GNU General Public License 13 : along with this program. If not, see <http://www.gnu.org/licenses/>. 14 : */ 15 : #pragma once 16 : #include "AP_Follow_config.h" 17 : 18 : #if AP_FOLLOW_ENABLED 19 : 20 : #include <AP_Common/AP_Common.h> 21 : #include <AP_Common/Location.h> 22 : #include <AP_HAL/AP_HAL.h> 23 : #include <AP_Param/AP_Param.h> 24 : #include <AP_Math/AP_Math.h> 25 : #include <GCS_MAVLink/GCS_MAVLink.h> 26 : #include <AC_PID/AC_P.h> 27 : #include <AP_RTC/JitterCorrection.h> 28 : 29 : //============================================================================== 30 : // AP_Follow Class 31 : // Target Following Logic for ArduPilot Vehicles 32 : //============================================================================== 33 : 34 : class AP_Follow 35 : { 36 : 37 : public: 38 : //========================================================================== 39 : // Public Enums 40 : //========================================================================== 41 : 42 : // enum for FOLLOW_OPTIONS parameter 43 : enum class Option { 44 : MOUNT_FOLLOW_ON_ENTER = 1 45 : }; 46 : 47 : // enum for YAW_BEHAVE parameter 48 : enum YawBehave { 49 : YAW_BEHAVE_NONE = 0, 50 : YAW_BEHAVE_FACE_LEAD_VEHICLE = 1, 51 : YAW_BEHAVE_SAME_AS_LEAD_VEHICLE = 2, 52 : YAW_BEHAVE_DIR_OF_FLIGHT = 3 53 : }; 54 : 55 : //========================================================================== 56 : // Constructor and Singleton Access 57 : //========================================================================== 58 : 59 : // constructor 60 : AP_Follow(); 61 : 62 : // enable as singleton 63 2645702 : static AP_Follow *get_singleton(void) { 64 2645702 : return _singleton; 65 : } 66 : 67 : // returns true if library is enabled 68 87756 : bool enabled() const { return _enabled; } 69 : 70 : // set which target to follow 71 0 : void set_target_sysid(uint8_t sysid) { _sysid.set(sysid); } 72 : 73 : // Resets the follow mode offsets to zero if they were automatically initialized. Should be called when exiting Follow mode. 74 : void clear_offsets_if_required(); 75 : 76 : //========================================================================== 77 : // Target Estimation and Tracking Methods 78 : //========================================================================== 79 : 80 : // Returns true if a usable estimate of the configured target is available. Every accessor 81 : // below is gated on this, but a true return does not guarantee they will succeed, so callers 82 : // must still check what each accessor returns. 83 : bool have_target() const; 84 : 85 : // Projects the target’s position, velocity, and heading forward using the latest updates, smoothing with input shaping if necessary 86 : void update_estimates(); 87 : 88 : // Retrieves the estimated target position, velocity, and acceleration in the NED frame relative to the origin (units: meters and meters/second). 89 : bool get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss) const; 90 : 91 : // Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured positional offsets. 92 : bool get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss) const; 93 : 94 : // Retrieves the estimated target heading and heading rate in radians. 95 : bool get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads) const; 96 : 97 : //========================================================================== 98 : // Global Location and Velocity Retrieval (LUA Bindings) 99 : //========================================================================== 100 : 101 : // Retrieves the estimated global location and velocity of the target 102 : bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned); 103 : 104 : // Retrieves the estimated global location including configured positional offsets and velocity of the target, (for LUA bindings). 105 : bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned); 106 : 107 : // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings. 108 : bool get_target_heading_deg(float &heading); 109 : 110 : // Retrieves the estimated target heading rate in degrees per second. 111 : bool get_target_heading_rate_degs(float &_target_heading_rate_degs); 112 : 113 : // Retrieves the distance vector to the target, the distance vector including configured offsets, and the target’s velocity in the NED frame (units: meters). 114 : bool get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned); 115 : 116 : //========================================================================== 117 : // Accessor Methods 118 : //========================================================================== 119 : 120 : // get target sysid as a 32 bit number to allow for future expansion of MAV_SYSID 121 7 : uint32_t get_target_sysid() const { return (uint32_t)_sysid.get(); } 122 : 123 : // get position controller. this controller is not used within this library but it is convenient to hold it here 124 0 : const AC_P& get_pos_p() const { return _p_pos; } 125 : 126 : // get user defined yaw behaviour 127 15272 : YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); } 128 : 129 : // initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required 130 : void init_offsets_if_required(); 131 : 132 : //========================================================================== 133 : // MAVLink Message Handling 134 : //========================================================================== 135 : 136 : // parse mavlink messages which may hold target's position, velocity and attitude 137 : void handle_msg(const mavlink_message_t &msg); 138 : 139 : //========================================================================== 140 : // GCS Reporting Methods 141 : //========================================================================== 142 : 143 : // get horizontal distance to target (including offset) in meters (for reporting purposes) 144 243 : float get_distance_to_target_m() const { return _dist_to_target_m; } 145 : 146 : // get bearing to target (including offset) in degrees (for reporting purposes) 147 243 : float get_bearing_to_target_deg() const { return _bearing_to_target_deg; } 148 : 149 : // get system time of last position update 150 : // LUA bindings 151 0 : uint32_t get_last_update_ms() const { return _last_location_update_ms; } 152 : 153 : // returns true if a follow option enabled 154 7 : bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; } 155 : 156 : //========================================================================== 157 : // Parameter Group Info 158 : //========================================================================== 159 : 160 : // parameter list 161 : static const struct AP_Param::GroupInfo var_info[]; 162 : 163 : private: 164 : //========================================================================== 165 : // Private Helper Functions and Singleton 166 : //========================================================================== 167 : 168 : // Singleton instance of the AP_Follow class. 169 : static AP_Follow *_singleton; 170 : 171 : // returns true if we should extract information from msg 172 : bool should_handle_message(const mavlink_message_t &msg) const; 173 : 174 : // Returns true if the target data we hold is fresh and was supplied by the configured system. 175 : bool have_target_data() const; 176 : 177 : // Checks whether the current estimate should be reset based on position and velocity errors. 178 : bool estimate_error_too_large() const; 179 : 180 : // Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited). 181 : float calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const; 182 : 183 : // Rotates a 3D vector clockwise by the specified angle in degrees. 184 : Vector3f rotate_vector(const Vector3f &vec, float angle_deg) const; 185 : 186 : // Resets the recorded distance and bearing to the target to zero. 187 : void clear_dist_and_bearing_to_target(); 188 : void update_dist_and_bearing_to_target(); 189 : 190 : // handle various mavlink messages supplying position: 191 : bool handle_global_position_int_message(const mavlink_message_t &msg); 192 : bool handle_follow_target_message(const mavlink_message_t &msg); 193 : 194 : // write out an onboard-log message to help diagnose follow problems: 195 : void Log_Write_FOLL(); 196 : 197 : //========================================================================== 198 : // Parameters 199 : //========================================================================== 200 : 201 : AP_Int8 _enabled; // 1 = Follow mode is enabled; 0 = disabled 202 : AP_Int16 _sysid; // MAVLink system ID of the target (0 = no target selected) 203 : AP_Float _dist_max_m; // Maximum allowed distance to target in meters; if exceeded, estimation is rejected 204 : AP_Int8 _offset_type; // Offset frame type: 0 = NED, 1 = relative to lead vehicle heading 205 : AP_Vector3f _offset_m; // Offset from lead vehicle (meters), in NED or FRD frame depending on _offset_type 206 : AP_Int8 _yaw_behave; // Yaw behavior mode (see YawBehave enum) 207 : AP_Enum<Location::AltFrame> _alt_type; // altitude source for follow mode 208 : AC_P _p_pos; // Position error P-controller for optional altitude following 209 : AP_Int16 _options; // Bitmask of follow behavior options (e.g., mount follow, etc.) 210 : AP_Float _timeout; // position estimate timeout after x milliseconds 211 : 212 : AP_Float _accel_max_ne_mss; // Max horizontal acceleration for kinematic shaping (m/s²) 213 : AP_Float _jerk_max_ne_msss; // Max horizontal jerk for kinematic shaping (m/s³) 214 : AP_Float _accel_max_d_mss; // Max vertical acceleration for kinematic shaping (m/s²) 215 : AP_Float _jerk_max_d_msss; // Max vertical jerk for kinematic shaping (m/s³) 216 : AP_Float _accel_max_h_degss; // Max angular acceleration for heading shaping (deg/s²) 217 : AP_Float _jerk_max_h_degsss; // Max angular jerk for heading shaping (deg/s³) 218 : 219 : //========================================================================== 220 : // Internal State Variables 221 : //========================================================================== 222 : 223 : uint32_t _last_location_update_ms; // Time of last target position update (ms) 224 : uint32_t _last_estimation_update_ms; // Time of last estimate update (ms) 225 : 226 : Vector3p _target_pos_ned_m; // Latest received target position (NED frame, meters) 227 : Vector3f _target_vel_ned_ms; // Latest received target velocity (NED frame, m/s) 228 : Vector3f _target_accel_ned_mss; // Latest received target acceleration (NED frame, m/s²) 229 : float _target_heading_deg; // Latest received target heading (degrees, 0 = North) 230 : float _target_heading_rate_degs; // Latest received target yaw rate (deg/s) 231 : 232 : bool _estimate_valid; // True if internal estimate is valid and in sync with updates 233 : Vector3p _estimate_pos_ned_m; // Estimated target position (NED frame, meters) 234 : Vector3f _estimate_vel_ned_ms; // Estimated target velocity (NED frame, m/s) 235 : Vector3f _estimate_accel_ned_mss; // Estimated target acceleration (NED frame, m/s²) 236 : float _estimate_heading_rad; // Estimated heading (radians, range -PI to PI) 237 : float _estimate_heading_rate_rads; // Estimated heading rate (rad/s) 238 : float _estimate_heading_accel_radss; // Estimated heading acceleration (rad/s²) 239 : 240 : Vector3p _ofs_estimate_pos_ned_m; // Estimated position with offsets applied (NED frame) 241 : Vector3f _ofs_estimate_vel_ned_ms; // Estimated velocity with offsets applied (NED frame) 242 : Vector3f _ofs_estimate_accel_ned_mss; // Estimated acceleration with offsets applied (NED frame) 243 : 244 : int16_t _sysid_of_data; // sysid that supplied the target data we currently hold 245 : float _dist_to_target_m; // Horizontal distance to target, for reporting (meters) 246 : float _bearing_to_target_deg; // Bearing to target from vehicle (degrees, 0 = North) 247 : bool _offsets_were_zero; // True if initial offset was zero before being initialized 248 : bool _using_follow_target; // True if FOLLOW_TARGET messages are being used instead of GLOBAL_POSITION_INT 249 : 250 : HAL_Semaphore _follow_sem; // semaphore for multi-thread use of update_estimates and LUA calls 251 : 252 : //========================================================================== 253 : // Utilities 254 : //========================================================================== 255 : 256 : // Jitter correction helper for smoothing offboard timestamps. 257 : JitterCorrection _jitter{500}; 258 : }; 259 : 260 : //============================================================================== 261 : // AP_Follow Accessor 262 : //============================================================================== 263 : 264 : namespace AP { 265 : AP_Follow &follow(); 266 : }; 267 : 268 : #endif
