Line data Source code
1 : #pragma once 2 : 3 : #include <AP_Common/AP_Common.h> 4 : #include <AP_Param/AP_Param.h> 5 : #include <AP_Math/AP_Math.h> 6 : #include <AP_Math/SCurve.h> 7 : #include <AP_Math/SplineCurve.h> 8 : #include <AP_Common/Location.h> 9 : #include <AC_AttitudeControl/AC_PosControl.h> // Position control library 10 : #include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude control library 11 : #include <AP_Terrain/AP_Terrain.h> 12 : #include <AC_Avoidance/AC_Avoid.h> // Stop at fence library 13 : 14 : // maximum velocities and accelerations 15 : #define WPNAV_ACCELERATION_MS 2.5 // default horizontal acceleration limit for waypoint navigation (m/s²) 16 : 17 : class AC_WPNav 18 : { 19 : public: 20 : 21 : /// Constructor 22 : AC_WPNav(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); 23 : 24 : // Sets terrain offset in cm from EKF origin using rangefinder. 25 : // See set_rangefinder_terrain_U_m() for full details. 26 109016 : void set_rangefinder_terrain_U_cm(bool use, bool healthy, float terrain_u_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_u_m = terrain_u_cm * 0.01;} 27 : 28 : // Sets terrain offset in meters from EKF origin using rangefinder data. 29 : // This value is used to determine alt-above-terrain for terrain following. 30 30392688 : void set_rangefinder_terrain_U_m(bool use, bool healthy, float terrain_u_m) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_u_m = terrain_u_m;} 31 : 32 : // Returns true if rangefinder is enabled and may be used for terrain following. 33 2121697 : bool rangefinder_used() const { return _rangefinder_use; } 34 : 35 : // Returns true if rangefinder is enabled and currently healthy. 36 146 : bool rangefinder_used_and_healthy() const { return _rangefinder_use && _rangefinder_healthy; } 37 : 38 : // Returns the expected source of terrain data when using alt-above-terrain commands. 39 : // Used by systems like ModeRTL to determine which terrain provider is active. 40 : enum class TerrainSource { 41 : TERRAIN_UNAVAILABLE, 42 : TERRAIN_FROM_RANGEFINDER, 43 : TERRAIN_FROM_TERRAINDATABASE, 44 : }; 45 : AC_WPNav::TerrainSource get_terrain_source() const; 46 : 47 : // Returns terrain offset in meters above the EKF origin at the current position. 48 : // Positive values mean terrain lies above the EKF origin altitude. 49 : // Source may be rangefinder or terrain database depending on availability. 50 : bool get_terrain_U_m(float& terrain_u_m); 51 : bool get_terrain_D_m(float& terrain_d_m); 52 : 53 : // Converts _origin_ned_m.z between the terrain-relative and origin-relative altitude frames 54 : // and re-seeds the position controller's terrain offset to match. Call only when the new 55 : // leg's frame differs from the current one. 56 : // Returns false if terrain data is required but unavailable. 57 : bool convert_origin_to_alt_frame(bool is_terrain_alt); 58 : 59 : // Returns the terrain following altitude margin in meters. 60 : // Vehicle will stop if distance from target altitude exceeds this margin. 61 4065764 : float get_terrain_margin_m() const { return MAX(_terrain_margin_m, 0.1); } 62 : 63 : // Converts a Location to a NED position vector in meters from the EKF origin. 64 : // Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain. 65 : // Returns false if terrain data is unavailable or conversion fails. 66 : bool get_vector_NED_m(const Location &loc, Vector3p &pos_from_origin_ned_m, bool &is_terrain_alt); 67 : 68 : /// 69 : /// waypoint controller 70 : /// 71 : 72 : // Initializes waypoint and spline navigation using inputs in meters. 73 : // Sets speed and acceleration limits, calculates jerk constraints, 74 : // and initializes spline or S-curve leg with a defined starting point. 75 : void wp_and_spline_init_m(float speed_ms = 0.0f, Vector3p stopping_point_ned_m = Vector3p{}); 76 : 77 : // Sets the target horizontal speed in cm/s during waypoint navigation. 78 : // See set_speed_NE_ms() for full details. 79 : void set_speed_NE_cms(float speed_cms); 80 : 81 : // Sets the target horizontal speed in m/s during waypoint navigation. 82 : // Also updates internal velocity offsets and path shaping limits. 83 : void set_speed_NE_ms(float speed_ms); 84 : 85 : // Pauses progression along the waypoint track. 86 4 : void set_pause() { _paused = true; } 87 : 88 : // Resumes waypoint navigation after a pause. Track advancement continues from the current position. 89 4 : void set_resume() { _paused = false; } 90 : 91 : // Returns true if waypoint navigation is currently paused via set_pause(). 92 386004 : bool paused() { return _paused; } 93 : 94 : // Sets the climb speed for waypoint navigation in m/s. 95 : // Updates the vertical controller with the new ascent rate limit. 96 : void set_speed_up_ms(float speed_up_ms); 97 : 98 : // Sets the descent speed for waypoint navigation in m/s. 99 : // Updates the vertical controller with the new descent rate limit. 100 : void set_speed_down_ms(float speed_down_ms); 101 : 102 : // Returns the default horizontal speed in cm/s used during waypoint navigation. 103 : // See get_default_speed_NE_ms() for full details. 104 2940 : float get_default_speed_NE_cms() const { return get_default_speed_NE_ms() * 100.0; } 105 : 106 : // Returns the default horizontal speed in m/s used during waypoint navigation. 107 : // Derived from the WP_SPD parameter. 108 2029427 : float get_default_speed_NE_ms() const { return _wp_speed_ms; } 109 : 110 : // Returns the default climb speed in cm/s used during waypoint navigation. 111 : // See get_default_speed_up_ms() for full details. 112 11 : float get_default_speed_up_cms() const { return get_default_speed_up_ms() * 100.0; } 113 : 114 : // Returns the default climb speed in m/s used during waypoint navigation. 115 : // Derived from the WP_SPD_UP parameter. 116 1466349 : float get_default_speed_up_ms() const { return _wp_speed_up_ms; } 117 : 118 : // Returns the default descent rate in cm/s used during waypoint navigation. 119 : // Always positive. See get_default_speed_down_ms() for full details. 120 6055 : float get_default_speed_down_cms() const { return get_default_speed_down_ms() * 100.0; } 121 : 122 : // Returns the default descent rate in m/s used during waypoint navigation. 123 : // Derived from the WP_SPD_DN parameter. Always positive. 124 1211831 : float get_default_speed_down_ms() const { return fabsf(_wp_speed_down_ms); } 125 : 126 : // Returns the vertical acceleration in cm/s² used during waypoint navigation. 127 : // Always positive. See get_accel_D_mss() for full details. 128 8 : float get_accel_D_cmss() const { return get_accel_D_mss() * 100.0; } 129 : 130 : // Returns the vertical acceleration in m/s² used during waypoint navigation. 131 : // Derived from the WP_ACC_Z parameter. Always positive. 132 32116 : float get_accel_D_mss() const { return _wp_accel_z_mss; } 133 : 134 : // Returns the horizontal acceleration in cm/s² used during waypoint navigation. 135 : // See get_wp_acceleration_mss() for full details. 136 2940 : float get_wp_acceleration_cmss() const { return get_wp_acceleration_mss() * 100.0; } 137 : 138 : // Returns the horizontal acceleration in m/s² used during waypoint navigation. 139 : // Derived from the WP_ACC parameter. Falls back to a default if unset. 140 21737994 : float get_wp_acceleration_mss() const { return (is_positive(_wp_accel_mss)) ? _wp_accel_mss : WPNAV_ACCELERATION_MS; } 141 : 142 : // Returns the maximum lateral acceleration in m/s² used during waypoint cornering. 143 : // Derived from WP_ACC_C or defaults to 2x WP_ACC if unset. 144 3902370 : float get_corner_acceleration_mss() const { return (is_positive(_wp_accel_c_mss)) ? _wp_accel_c_mss : 2.0 * get_wp_acceleration_mss(); } 145 : 146 : // Returns the destination waypoint vector in NEU frame, in centimeters from EKF origin. 147 : // See get_wp_destination_NED_m() for full details. 148 398817 : const Vector3f get_wp_destination_NEU_cm() const { return Vector3f(_destination_ned_m.x, _destination_ned_m.y, -_destination_ned_m.z) * 100.0; } 149 : 150 : // Returns the destination waypoint vector in NED frame, in meters from EKF origin. 151 : // Z is relative to terrain or EKF origin, depending on _is_terrain_alt. 152 60093 : const Vector3p &get_wp_destination_NED_m() const { return _destination_ned_m; } 153 : 154 : // Returns the origin waypoint vector in NEU frame, in centimeters from EKF origin. 155 : // See get_wp_origin_NED_m() for full details. 156 398817 : const Vector3f get_wp_origin_NEU_cm() const { return Vector3f(_origin_ned_m.x, _origin_ned_m.y, -_origin_ned_m.z) * 100.0; } 157 : 158 : // Returns the origin waypoint vector in NED frame, in meters from EKF origin. 159 : // This marks the start of the current waypoint leg. 160 : const Vector3p &get_wp_origin_NED_m() const { return _origin_ned_m; } 161 : 162 : // Returns true if origin.z and destination.z are specified as altitudes above terrain. 163 : // Returns false if altitudes are relative to the EKF origin. 164 0 : bool origin_and_destination_are_terrain_alt() const { return _is_terrain_alt; } 165 : 166 : // Sets the current waypoint destination using a Location object. 167 : // Converts global coordinates to NED position and sets destination. 168 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) 169 : // Returns false if conversion fails (e.g. missing terrain data). 170 : bool set_wp_destination_loc(const Location& destination, float arc_rad = 0.0); 171 : 172 : // Sets the next waypoint destination using a Location object. 173 : // Converts global coordinates to NED position and preloads the trajectory. 174 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) 175 : // Returns false if conversion fails or terrain data is unavailable. 176 : bool set_wp_destination_next_loc(const Location& destination, float arc_rad = 0.0); 177 : 178 : // Gets the current waypoint destination as a Location object. 179 : // Altitude frame will be ABOVE_TERRAIN or ABOVE_ORIGIN depending on path configuration. 180 : // Returns false if origin is not set or coordinate conversion fails. 181 : bool get_wp_destination_loc(Location& destination) const; 182 : 183 : // Returns the waypoint destination adjusted for object avoidance. 184 : // In the base class this is identical to get_wp_destination_loc(). 185 : // Used to unify the AC_WPNav and AC_WPNav_OA interfaces. 186 0 : virtual bool get_oa_wp_destination(Location& destination) const { return get_wp_destination_loc(destination); } 187 : 188 : // Sets waypoint destination using NEU position vector in centimeters from EKF origin. 189 : // See set_wp_destination_NED_m() for full details. 190 : virtual bool set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt = false); 191 : 192 : // Sets waypoint destination using NED position vector in meters from EKF origin. 193 : // If `is_terrain_alt` is true, altitude is interpreted as height above terrain. 194 : // Reinitializes the current leg if interrupted, updates origin, and computes trajectory. 195 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) 196 : // Returns false if terrain offset cannot be determined when required. 197 : virtual bool set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt = false, float arc_rad = 0.0); 198 : 199 : // Sets the next waypoint destination using a NED position vector in meters. 200 : // Only updates if terrain frame matches current leg. 201 : // Calculates trajectory preview for smoother transition into next segment. 202 : // Updates velocity handoff if previous leg is a spline. 203 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path) 204 : bool set_wp_destination_next_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt = false, float arc_rad = 0.0); 205 : 206 : // Sets a circular-orbit destination about center_ne_m using the S-curve engine. 207 : // The current destination (used as the leg origin) must lie on the circle; the radius is the 208 : // distance from that origin to the center. turns_signed is the signed number of turns (its sign 209 : // selects direction and its magnitude may exceed 1 for multiple turns). dest_d_m is the altitude 210 : // (NED down) at the end of the orbit and is_terrain_alt selects the altitude frame. The orbit is 211 : // flown at the waypoint speed, limited by the corner acceleration through the arc radius. 212 : // Returns false if a terrain-frame transition is required but terrain data is unavailable. 213 : // Not virtual: AC_WPNav_OA holds its avoidance state inactive for the whole orbit in 214 : // update_wpnav() instead of overriding here, which lets the linker drop this and the 215 : // S-curve arc builder behind it from vehicles that never fly an orbit. 216 : bool set_circle_destination_NED_m(const Vector2f& center_ne_m, float turns_signed, float dest_d_m, bool is_terrain_alt); 217 : 218 : // Returns the angle swept around the current circular-orbit leg so far, in radians. 219 : // Unsigned and unwrapped, so it grows past 2*pi on a multi-turn orbit. 220 : // Returns zero unless the current leg was set by set_circle_destination_NED_m(). 221 46670 : float get_circle_angle_covered_rad() const { return _this_leg_is_circle ? _scurve_this_leg.get_arc_angle_covered_rad() : 0.0; } 222 : 223 : // Computes the horizontal stopping point in NE frame, returned in centimeters. 224 : // See get_wp_stopping_point_NE_m() for full details. 225 : void get_wp_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const; 226 : 227 : // Computes the horizontal stopping point in NE frame, in meters, based on current velocity and configured acceleration. 228 : // This is the point where the vehicle would come to a stop if decelerated using the configured limits. 229 : void get_wp_stopping_point_NE_m(Vector2p& stopping_point_ne_m) const; 230 : 231 : // Computes the full 3D NEU stopping point vector in centimeters based on current kinematics. 232 : // See get_wp_stopping_point_NED_m() for full details. 233 : void get_wp_stopping_point_NEU_cm(Vector3f& stopping_point_neu_cm) const; 234 : 235 : // Computes the full 3D NED stopping point in meters based on current velocity and configured acceleration in all axes. 236 : // Represents where the vehicle will stop if decelerated from current velocity using configured limits. 237 : void get_wp_stopping_point_NED_m(Vector3p& stopping_point_ned_m) const; 238 : 239 : // Returns the horizontal distance to the destination waypoint in centimeters. 240 : // See get_wp_distance_to_destination_m() for full details. 241 : virtual float get_wp_distance_to_destination_cm() const; 242 : 243 : // Returns the horizontal distance in meters between the current position and the destination waypoint. 244 : virtual float get_wp_distance_to_destination_m() const; 245 : 246 : // Returns the bearing to the current waypoint destination in centidegrees. 247 : // See get_wp_bearing_to_destination_rad() for full details. 248 : virtual int32_t get_wp_bearing_to_destination_cd() const; 249 : 250 : // Returns the bearing to the current waypoint destination in radians. 251 : // The bearing is measured clockwise from North, with 0 = North. 252 : virtual float get_wp_bearing_to_destination_rad() const; 253 : 254 : // Returns true if the vehicle has reached the waypoint destination. 255 : // A waypoint is considered reached when the vehicle comes within the defined radius threshold. 256 3280303 : virtual bool reached_wp_destination() const { return _flags.reached_destination; } 257 : 258 : // Returns true if the vehicle's horizontal (NE) distance to the waypoint is less than the waypoint radius. 259 : // Uses the waypoint radius in meters for comparison. 260 3642 : bool reached_wp_destination_NE() const { 261 3642 : return get_wp_distance_to_destination_m() < _wp_radius_m; 262 : } 263 : 264 : // Returns the waypoint acceptance radius in meters. 265 : // This radius defines the distance from the target waypoint within which the vehicle is considered to have arrived. 266 885 : float get_wp_radius_m() const { return _wp_radius_m; } 267 : 268 : // perform any required parameter conversions 269 : void convert_parameters(); 270 : 271 : // Runs the waypoint navigation controller. 272 : // Advances the target position and updates the position controller. 273 : // Should be called at 100 Hz or higher for accurate tracking. 274 : virtual bool update_wpnav(); 275 : 276 : // Returns true if update_wpnav() has been called within the last 200 ms. 277 : // Used to check if waypoint navigation is currently active. 278 : bool is_active() const; 279 : 280 : // Forces a stop at the next waypoint instead of continuing to the subsequent one. 281 : // Used by Dijkstra’s object avoidance when the future path is obstructed. 282 : // Only affects regular (non-spline) waypoints. 283 : // Returns true if the stop behavior was newly enforced. 284 : bool force_stop_at_next_wp(); 285 : 286 : /// 287 : /// spline methods 288 : /// 289 : 290 : // Sets the current spline waypoint using global coordinates. 291 : // Converts `destination` and `next_destination` to NED position vectors and sets up a spline between them. 292 : // Returns false if conversion from location to vector fails. 293 : bool set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline); 294 : 295 : // Sets the next spline segment using global coordinates. 296 : // Converts the next and next-next destinations to NED position vectors and initializes the spline transition. 297 : // Returns false if any conversion from location to vector fails. 298 : bool set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline); 299 : 300 : // Sets the current spline waypoint using NED position vectors in meters. 301 : // Initializes a spline path from `destination_ned_m` to `next_destination_ned_m`, respecting terrain altitude framing. 302 : // Both waypoints must use the same altitude frame (either above terrain or above origin). 303 : // Returns false if terrain altitude cannot be determined when required. 304 : bool set_spline_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, const Vector3p& next_destination_ned_m, bool next_terrain_alt, bool next_is_spline); 305 : 306 : // Sets the next spline segment using NED position vectors in meters. 307 : // Creates a spline path from the current destination to `next_destination_ned_m`, and prepares transition toward `next_next_destination_ned_m`. 308 : // All waypoints must use the same altitude frame (above terrain or origin). 309 : // Returns false if terrain data is missing and required. 310 : bool set_spline_destination_next_NED_m(const Vector3p& next_destination_ned_m, bool next_is_terrain_alt, const Vector3p& next_next_destination_ned_m, bool next_next_is_terrain_alt, bool next_next_is_spline); 311 : 312 : /// 313 : /// shared methods 314 : /// 315 : 316 : // Returns the desired roll angle in radians from the position controller. 317 : // This value is passed to the attitude controller to achieve lateral motion. 318 589364 : float get_roll_rad() const { return _pos_control.get_roll_rad(); } 319 : 320 : // Returns the desired pitch angle in radians from the position controller. 321 : // This angle controls forward or backward acceleration. 322 589364 : float get_pitch_rad() const { return _pos_control.get_pitch_rad(); } 323 : 324 : // Returns the desired yaw angle in radians from the position controller. 325 : // Used to align the vehicle heading with track direction or command input. 326 26559 : float get_yaw_rad() const { return _pos_control.get_yaw_rad(); } 327 : 328 : // Returns the desired 3D unit vector representing the commanded thrust direction. 329 : // Used to calculate tilt angles for the attitude controller. 330 : Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); } 331 : 332 : // Returns the desired roll angle in centidegrees from the position controller. 333 : // See get_roll_rad() for full details. 334 523557 : float get_roll() const { return rad_to_cd(get_roll_rad()); } 335 : 336 : // Returns the desired pitch angle in centidegrees from the position controller. 337 : // See get_pitch_rad() for full details. 338 523557 : float get_pitch() const { return rad_to_cd(get_pitch_rad()); } 339 : 340 : // Returns the desired yaw angle in centidegrees from the position controller. 341 : // See get_yaw_rad() for full details. 342 26559 : float get_yaw() const { return rad_to_cd(get_yaw_rad()); } 343 : 344 : // Advances the target location along the current path segment. 345 : // Updates target position, velocity, and acceleration based on jerk-limited profile (or spline). 346 : // Returns true if the update succeeded (e.g., terrain data was available). 347 : bool advance_wp_target_along_track(float dt); 348 : 349 : // Updates the current and next path segment to reflect new speed and acceleration limits. 350 : // Should be called after modifying NE/U controller limits or vehicle configuration. 351 : void update_track_with_speed_accel_limits(); 352 : 353 : // Returns lateral (cross-track) position error in meters. 354 : // Computed as the perpendicular distance between current position and the planned path. 355 : // Used to assess horizontal deviation from the trajectory. 356 154145 : float crosstrack_error_m() const { return _pos_control.crosstrack_error_m();} 357 : 358 : static const struct AP_Param::GroupInfo var_info[]; 359 : 360 : protected: 361 : 362 : // flags structure 363 : struct wpnav_flags { 364 : uint8_t reached_destination : 1; // true if we have reached the destination 365 : uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint 366 : uint8_t wp_yaw_set : 1; // true if yaw target has been set 367 : } _flags; 368 : 369 : // Calculates s-curve jerk and snap limits based on attitude controller capabilities. 370 : // Updates _scurve_jerk_max_msss and _scurve_snap_max_mssss with constrained values. 371 : void calc_scurve_jerk_and_snap(); 372 : 373 : // References to shared sensor fusion, position, and attitude control subsystems. 374 : const AP_AHRS_View& _ahrs; 375 : AC_PosControl& _pos_control; 376 : const AC_AttitudeControl& _attitude_control; 377 : 378 : // parameters 379 : AP_Float _wp_speed_ms; // default horizontal speed in m/s for waypoint navigation 380 : AP_Float _wp_speed_up_ms; // default climb rate in m/s for waypoint navigation 381 : AP_Float _wp_speed_down_ms; // default descent rate in m/s for waypoint navigation 382 : AP_Float _wp_radius_m; // waypoint radius in m; waypoint is considered reached when within this distance 383 : AP_Float _wp_accel_mss; // maximum horizontal acceleration in m/s² used during waypoint tracking 384 : AP_Float _wp_accel_c_mss; // maximum acceleration in m/s² for turns; defaults to 2x horizontal accel if unset 385 : AP_Float _wp_accel_z_mss; // maximum vertical acceleration in m/s² used during climb or descent 386 : AP_Float _wp_jerk_msss; // maximum jerk in m/s³ used for s-curve trajectory shaping 387 : AP_Float _terrain_margin_m; // minimum altitude margin in meters when terrain following is active 388 : 389 : // WP_SPD param change checker 390 : bool _check_wp_speed_change; // true if WP_SPD should be monitored for changes during flight 391 : float _last_wp_speed_ms; // last recorded WP_SPD value (m/s) for change detection 392 : float _last_wp_speed_up_ms; // last recorded WP_SPD_UP value (m/s) 393 : float _last_wp_speed_down_ms; // last recorded WP_SPD_DN value (m/s) 394 : 395 : // s-curve trajectory objects 396 : SCurve _scurve_prev_leg; // s-curve for the previous waypoint leg, used for smoothing transitions 397 : SCurve _scurve_this_leg; // s-curve for the current active waypoint leg 398 : SCurve _scurve_next_leg; // s-curve for the next waypoint leg, used for lookahead blending 399 : float _scurve_jerk_max_msss; // computed maximum jerk in m/s³ used for trajectory shaping 400 : float _scurve_snap_max_mssss; // computed maximum snap in m/s⁴ derived from controller responsiveness 401 : 402 : // spline curves 403 : SplineCurve _spline_this_leg; // spline curve for the current segment 404 : SplineCurve _spline_next_leg; // spline curve for the next segment 405 : 406 : // path type flags 407 : bool _this_leg_is_spline; // true if the current leg uses spline trajectory 408 : bool _next_leg_is_spline; // true if the next leg will use spline trajectory 409 : bool _this_leg_is_circle; // true if the current leg is a circular orbit built by set_circle_destination_NED_m 410 : 411 : // waypoint navigation state 412 : uint32_t _wp_last_update_ms; // timestamp of the last update_wpnav() call (milliseconds) 413 : float _wp_desired_speed_ne_ms; // desired horizontal speed in m/s for the current segment 414 : Vector3p _origin_ned_m; // origin of the current leg in meters (NED frame) 415 : Vector3p _destination_ned_m; // destination of the current leg in meters (NED frame) 416 : Vector3p _next_destination_ned_m; // destination of the next leg in meters (NED frame) 417 : float _track_dt_scalar; // scalar to reduce or increase the advancement along the track (0.0–1.0) 418 : float _offset_vel_ms; // filtered horizontal speed target (used for terrain following or pause handling) 419 : float _offset_accel_mss; // filtered horizontal acceleration target (used for terrain following or pause handling) 420 : bool _paused; // true if waypoint controller is paused 421 : 422 : // terrain following state 423 : bool _is_terrain_alt; // true if altitude values are relative to terrain height, false if relative to EKF origin 424 : bool _rangefinder_available; // true if a rangefinder is enabled and available for use 425 : AP_Int8 _rangefinder_use; // parameter specifying whether rangefinder should be used for terrain tracking 426 : bool _rangefinder_healthy; // true if the rangefinder reading is valid and within operational range 427 : float _rangefinder_terrain_u_m; // rangefinder-derived terrain offset (meters above EKF origin) 428 : };
