Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include <AP_Vehicle/AP_Vehicle_Type.h>
3 : #include "AC_WPNav.h"
4 :
5 : extern const AP_HAL::HAL& hal;
6 :
7 : // Default waypoint navigation constraints
8 : #define WP_SPD_DEFAULT 10.0f // default horizontal speed between waypoints (m/s)
9 : #define WP_SPD_MIN 0.01f // minimum horizontal speed allowed (m/s)
10 : #define WP_RADIUS_M_DEFAULT 2.0f // default radius within which a waypoint is considered reached (m)
11 : #define WP_RADIUS_M_MIN 0.05f // minimum allowable waypoint radius (m)
12 : #define WP_SPD_UP_DEFAULT 2.5f // default maximum climb speed (m/s)
13 : #define WP_SPD_DOWN_DEFAULT 1.5f // default maximum descent speed (m/s)
14 : #define WP_ACC_Z_DEFAULT 1.0f // default vertical acceleration limit (m/s²)
15 :
16 : const AP_Param::GroupInfo AC_WPNav::var_info[] = {
17 : // 0 was SPEED
18 : // 1 was RADIUS
19 : // 2 was SPEED_UP
20 : // 3 was SPEED_DN
21 : // 5 was ACCEL
22 : // 6 was ACCEL_Z
23 :
24 : // @Param: RFND_USE
25 : // @DisplayName: Waypoint missions use rangefinder for terrain following
26 : // @Description: This controls if waypoint missions use rangefinder for terrain following
27 : // @Values: 0:Disable,1:Enable
28 : // @User: Advanced
29 : AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1),
30 :
31 : // @Param: JERK
32 : // @DisplayName: Waypoint Jerk
33 : // @Description: Defines the horizontal jerk in m/s/s used during missions
34 : // @Units: m/s/s/s
35 : // @Range: 1 20
36 : // @User: Standard
37 : AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk_msss, 1.0f),
38 :
39 : // @Param: TER_MARGIN
40 : // @DisplayName: Waypoint Terrain following altitude margin
41 : // @Description: Waypoint Terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters)
42 : // @Units: m
43 : // @Range: 0.1 100
44 : // @User: Advanced
45 : AP_GROUPINFO("TER_MARGIN", 12, AC_WPNav, _terrain_margin_m, 10.0),
46 :
47 : // 13 was ACCEL_C
48 :
49 : // @Param: SPD
50 : // @DisplayName: Waypoint Horizontal Speed Target
51 : // @Description: Speed in m/s which the aircraft will attempt to maintain horizontally during a WP mission
52 : // @Units: m/s
53 : // @Range: 0.10 20.00
54 : // @Increment: 0.10
55 : // @User: Standard
56 : AP_GROUPINFO("SPD", 14, AC_WPNav, _wp_speed_ms, WP_SPD_DEFAULT),
57 :
58 : // @Param: RADIUS_M
59 : // @DisplayName: Waypoint Radius
60 : // @Description: Distance from a waypoint, that when crossed indicates the wp has been reached.
61 : // @Units: m
62 : // @Range: 0.05 10.00
63 : // @Increment: 0.01
64 : // @User: Standard
65 : AP_GROUPINFO("RADIUS_M", 15, AC_WPNav, _wp_radius_m, WP_RADIUS_M_DEFAULT),
66 :
67 : // @Param: SPD_UP
68 : // @DisplayName: Waypoint Climb Speed Target
69 : // @Description: Speed in m/s which the aircraft will attempt to maintain while climbing during a WP mission
70 : // @Units: m/s
71 : // @Range: 0.10 10.00
72 : // @Increment: 0.10
73 : // @User: Standard
74 : AP_GROUPINFO("SPD_UP", 16, AC_WPNav, _wp_speed_up_ms, WP_SPD_UP_DEFAULT),
75 :
76 : // @Param: SPD_DN
77 : // @DisplayName: Waypoint Descent Speed Target
78 : // @Description: Speed in m/s which the aircraft will attempt to maintain while descending during a WP mission
79 : // @Units: m/s
80 : // @Range: 0.10 10.00
81 : // @Increment: 0.10
82 : // @User: Standard
83 : AP_GROUPINFO("SPD_DN", 17, AC_WPNav, _wp_speed_down_ms, WP_SPD_DOWN_DEFAULT),
84 :
85 : // @Param: ACC
86 : // @DisplayName: Waypoint Acceleration
87 : // @Description: Horizontal acceleration in m/s/s used during missions
88 : // @Units: m/s/s
89 : // @Range: 0.50 5.00
90 : // @Increment: 0.10
91 : // @User: Standard
92 : AP_GROUPINFO("ACC", 18, AC_WPNav, _wp_accel_mss, WPNAV_ACCELERATION_MS),
93 :
94 : // @Param: ACC_Z
95 : // @DisplayName: Waypoint Vertical Acceleration
96 : // @Description: Vertical acceleration in m/s/s used during missions
97 : // @Units: m/s/s
98 : // @Range: 0.50 5.00
99 : // @Increment: 0.10
100 : // @User: Standard
101 : AP_GROUPINFO("ACC_Z", 19, AC_WPNav, _wp_accel_z_mss, WP_ACC_Z_DEFAULT),
102 :
103 : // @Param: ACC_CNR
104 : // @DisplayName: Waypoint Cornering Acceleration
105 : // @Description: Maximum cornering acceleration in m/s/s used during missions. If zero uses 2x accel value.
106 : // @Units: m/s/s
107 : // @Range: 0 5.00
108 : // @Increment: 0.10
109 : // @User: Standard
110 : AP_GROUPINFO("ACC_CNR", 20, AC_WPNav, _wp_accel_c_mss, 0.0),
111 :
112 : AP_GROUPEND
113 : };
114 :
115 : // perform any required parameter conversions
116 1285 : void AC_WPNav::convert_parameters()
117 : {
118 : // PARAMETER_CONVERSION - Added: Feb-2026 for ArduPilot-4.7
119 :
120 : // return immediately if no conversion is needed
121 3460 : if (_wp_speed_ms.configured() || _wp_radius_m.configured() || _wp_speed_up_ms.configured() ||
122 4532 : _wp_speed_down_ms.configured() || _wp_accel_mss.configured() || _wp_accel_z_mss.configured() ||
123 1072 : _wp_accel_c_mss.configured()) {
124 213 : return;
125 : }
126 :
127 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
128 : // convert QuadPlane parameters from cm to meters
129 : static const AP_Param::ConversionInfo conversion_info[] = {
130 : { 205, 4052, AP_PARAM_FLOAT, "Q_WP_SPD" }, // old Q_WP_SPEED at GroupElement 4052
131 : { 205, 84, AP_PARAM_FLOAT, "Q_WP_RADIUS_M" }, // old Q_WP_RADIUS at GroupElement 84
132 : { 205, 148, AP_PARAM_FLOAT, "Q_WP_SPD_UP" }, // old Q_WP_SPEED_UP at GroupElement 148
133 : { 205, 212, AP_PARAM_FLOAT, "Q_WP_SPD_DN" }, // old Q_WP_SPEED_DN at GroupElement 212
134 : { 205, 340, AP_PARAM_FLOAT, "Q_WP_ACC" }, // old Q_WP_ACCEL at GroupElement 340
135 : { 205, 404, AP_PARAM_FLOAT, "Q_WP_ACC_Z" }, // old Q_WP_ACCEL_Z at GroupElement 404
136 : { 205, 852, AP_PARAM_FLOAT, "Q_WP_ACC_CNR" }, // old Q_WP_ACCEL_C at GroupElement 852
137 : };
138 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
139 : // convert Sub parameters from cm to meters
140 : static const AP_Param::ConversionInfo conversion_info[] = {
141 : { 55, 0, AP_PARAM_FLOAT, "WP_SPD" }, // old WPNAV_SPEED at index 0
142 : { 55, 1, AP_PARAM_FLOAT, "WP_RADIUS_M" }, // old WPNAV_RADIUS at index 1
143 : { 55, 2, AP_PARAM_FLOAT, "WP_SPD_UP" }, // old WPNAV_SPEED_UP at index 2
144 : { 55, 3, AP_PARAM_FLOAT, "WP_SPD_DN" }, // old WPNAV_SPEED_DN at index 3
145 : { 55, 5, AP_PARAM_FLOAT, "WP_ACC" }, // old WPNAV_ACCEL at index 5
146 : { 55, 6, AP_PARAM_FLOAT, "WP_ACC_Z" }, // old WPNAV_ACCEL_Z at index 6
147 : { 55, 13, AP_PARAM_FLOAT, "WP_ACC_CNR" }, // old WPNAV_ACCEL_C at index 13
148 : };
149 : #else
150 : // convert Copter parameters from cm to meters
151 : static const AP_Param::ConversionInfo conversion_info[] = {
152 : { 101, 0, AP_PARAM_FLOAT, "WP_SPD" }, // old WPNAV_SPEED at index 0
153 : { 101, 1, AP_PARAM_FLOAT, "WP_RADIUS_M" }, // old WPNAV_RADIUS at index 1
154 : { 101, 2, AP_PARAM_FLOAT, "WP_SPD_UP" }, // old WPNAV_SPEED_UP at index 2
155 : { 101, 3, AP_PARAM_FLOAT, "WP_SPD_DN" }, // old WPNAV_SPEED_DN at index 3
156 : { 101, 5, AP_PARAM_FLOAT, "WP_ACC" }, // old WPNAV_ACCEL at index 5
157 : { 101, 6, AP_PARAM_FLOAT, "WP_ACC_Z" }, // old WPNAV_ACCEL_Z at index 6
158 : { 101, 13, AP_PARAM_FLOAT, "WP_ACC_CNR" }, // old WPNAV_ACCEL_C at index 13
159 : };
160 : #endif
161 :
162 1072 : AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
163 : }
164 :
165 : // Default constructor.
166 : // Note that the Vector/Matrix constructors already implicitly zero their values.
167 1285 : AC_WPNav::AC_WPNav(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
168 1285 : _ahrs(ahrs),
169 1285 : _pos_control(pos_control),
170 1285 : _attitude_control(attitude_control)
171 : {
172 1285 : AP_Param::setup_object_defaults(this, var_info);
173 :
174 : // init flags
175 1285 : _flags.reached_destination = false;
176 1285 : _flags.fast_waypoint = false;
177 :
178 : // record WP_SPD, WP_SPD_UP, WP_SPD_DN values so we can detect changes
179 1285 : _last_wp_speed_ms = _wp_speed_ms;
180 1285 : _last_wp_speed_up_ms = _wp_speed_up_ms;
181 1285 : _last_wp_speed_down_ms = get_default_speed_down_ms();
182 1285 : }
183 :
184 : // Returns the expected source of terrain data when using alt-above-terrain commands.
185 : // Used by systems like ModeRTL to determine which terrain provider is active.
186 425195 : AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
187 : {
188 : // use range finder if connected
189 : // prioritise rangefinder when enabled and available
190 425195 : if (_rangefinder_available && _rangefinder_use) {
191 191984 : return AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER;
192 : }
193 : #if AP_TERRAIN_AVAILABLE
194 233211 : const AP_Terrain *terrain = AP::terrain();
195 : // use terrain database if enabled
196 233211 : if (terrain != nullptr && terrain->enabled()) {
197 233054 : return AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE;
198 : } else {
199 157 : return AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE;
200 : }
201 : #else
202 : // fallback if no terrain support is compiled in
203 : return AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE;
204 : #endif
205 : }
206 :
207 : ///
208 : /// waypoint navigation
209 : ///
210 :
211 : // Initializes waypoint and spline navigation using inputs in meters.
212 : // Sets speed and acceleration limits, calculates jerk constraints,
213 : // and initializes spline or S-curve leg with a defined starting point.
214 15182 : void AC_WPNav::wp_and_spline_init_m(float speed_ms, Vector3p stopping_point_ned_m)
215 : {
216 : // ensure waypoint radius is not below minimum allowed value
217 15182 : _wp_radius_m.set_and_save_ifchanged(MAX(_wp_radius_m, WP_RADIUS_M_MIN));
218 :
219 : // ensure waypoint speed is not below minimum allowed value
220 15182 : _wp_speed_ms.set_and_save_ifchanged(MAX(_wp_speed_ms, WP_SPD_MIN));
221 :
222 : // initialise position controller
223 15182 : _pos_control.D_init_controller_stopping_point();
224 15182 : _pos_control.NE_init_controller_stopping_point();
225 :
226 : // determine desired waypoint speed; fallback to default if not provided
227 15182 : _check_wp_speed_change = !is_positive(speed_ms);
228 15182 : _wp_desired_speed_ne_ms = is_positive(speed_ms) ? speed_ms : get_default_speed_NE_ms();
229 15182 : _wp_desired_speed_ne_ms = MAX(_wp_desired_speed_ne_ms, WP_SPD_MIN);
230 :
231 : // initialise position controller speed and acceleration
232 15182 : _pos_control.NE_set_max_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss());
233 15182 : _pos_control.NE_set_correction_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss());
234 15182 : _pos_control.D_set_max_speed_accel_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_D_mss());
235 15182 : _pos_control.D_set_correction_speed_accel_m(get_default_speed_down_ms(), get_default_speed_up_ms(), get_accel_D_mss());
236 :
237 : // calculate jerk limit if not explicitly set by parameter
238 15182 : if (!is_positive(_wp_jerk_msss)) {
239 0 : _wp_jerk_msss.set(get_wp_acceleration_mss());
240 : }
241 15182 : calc_scurve_jerk_and_snap();
242 :
243 15182 : _scurve_prev_leg.init();
244 15182 : _scurve_this_leg.init();
245 15182 : _scurve_next_leg.init();
246 15182 : _track_dt_scalar = 1.0f;
247 :
248 15182 : _flags.reached_destination = true;
249 15182 : _flags.fast_waypoint = false;
250 :
251 : // determine initial origin and destination; fallback to current stopping point if not provided
252 15182 : if (stopping_point_ned_m.is_zero()) {
253 15125 : get_wp_stopping_point_NED_m(stopping_point_ned_m);
254 : }
255 15182 : _origin_ned_m = _destination_ned_m = stopping_point_ned_m;
256 15182 : _is_terrain_alt = false;
257 15182 : _this_leg_is_spline = false;
258 15182 : _this_leg_is_circle = false;
259 :
260 : // initialise velocity profile for terrain margin shaping
261 15182 : _offset_vel_ms = _wp_desired_speed_ne_ms;
262 15182 : _offset_accel_mss = 0.0;
263 15182 : _paused = false;
264 :
265 : // mark as active
266 15182 : _wp_last_update_ms = AP_HAL::millis();
267 15182 : }
268 :
269 : // Sets the target horizontal speed in cm/s during waypoint navigation.
270 : // See set_speed_NE_ms() for full details.
271 6 : void AC_WPNav::set_speed_NE_cms(float speed_cms)
272 : {
273 6 : set_speed_NE_ms(speed_cms * 0.01);
274 6 : }
275 :
276 : // Sets the target horizontal speed in m/s during waypoint navigation.
277 : // Also updates internal velocity offsets and path shaping limits.
278 667 : void AC_WPNav::set_speed_NE_ms(float speed_ms)
279 : {
280 : // validate input: speed must be above minimum and current desired speed must be non-zero
281 667 : if (speed_ms >= WP_SPD_MIN && is_positive(_wp_desired_speed_ne_ms)) {
282 : // adjust internal velocity offset to preserve relative motion scaling
283 667 : _offset_vel_ms = speed_ms * _offset_vel_ms / _wp_desired_speed_ne_ms;
284 :
285 : // set new desired horizontal speed for waypoint controller
286 667 : _wp_desired_speed_ne_ms = speed_ms;
287 :
288 : // update horizontal shaping and correction constraints
289 667 : _pos_control.NE_set_max_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss());
290 667 : _pos_control.NE_set_correction_speed_accel_m(_wp_desired_speed_ne_ms, get_wp_acceleration_mss());
291 :
292 : // update internal trajectory with new limits
293 667 : update_track_with_speed_accel_limits();
294 : }
295 667 : }
296 :
297 : // Sets the climb speed for waypoint navigation in m/s.
298 : // Updates the vertical controller with the new ascent rate limit.
299 11 : void AC_WPNav::set_speed_up_ms(float speed_up_ms)
300 : {
301 : // update vertical controller's max speed and accel limits (U axis)
302 11 : _pos_control.D_set_max_speed_accel_m(_pos_control.get_max_speed_down_ms(), speed_up_ms, _pos_control.D_get_max_accel_mss());
303 :
304 : // recompute the trajectory using updated limits
305 11 : update_track_with_speed_accel_limits();
306 11 : }
307 :
308 : // Sets the descent speed for waypoint navigation in m/s.
309 : // Updates the vertical controller with the new descent rate limit.
310 8 : void AC_WPNav::set_speed_down_ms(float speed_down_ms)
311 : {
312 : // update vertical controller descent speed
313 8 : _pos_control.D_set_max_speed_accel_m(speed_down_ms, _pos_control.get_max_speed_up_ms(), _pos_control.D_get_max_accel_mss());
314 :
315 : // recompute the trajectory using updated limits
316 8 : update_track_with_speed_accel_limits();
317 8 : }
318 :
319 : // Sets the current waypoint destination using a Location object.
320 : // Converts global coordinates to NED position and sets destination.
321 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path)
322 : // Returns false if conversion fails (e.g. missing terrain data).
323 850 : bool AC_WPNav::set_wp_destination_loc(const Location& destination, float arc_rad)
324 : {
325 : bool is_terrain_alt;
326 850 : Vector3p dest_ned_m;
327 :
328 : // convert Location to NED position vector in meters and determine altitude reference frame
329 850 : if (!get_vector_NED_m(destination, dest_ned_m, is_terrain_alt)) {
330 0 : return false;
331 : }
332 :
333 : // apply destination as the active waypoint leg
334 850 : return set_wp_destination_NED_m(dest_ned_m, is_terrain_alt, arc_rad);
335 : }
336 :
337 : // Sets the next waypoint destination using a Location object.
338 : // Converts global coordinates to NED position and preloads the trajectory.
339 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path)
340 : // Returns false if conversion fails or terrain data is unavailable.
341 446 : bool AC_WPNav::set_wp_destination_next_loc(const Location& destination, float arc_rad)
342 : {
343 : bool is_terrain_alt;
344 446 : Vector3p dest_ned_m;
345 :
346 : // convert Location to NED position vector in meters and determine altitude reference frame
347 446 : if (!get_vector_NED_m(destination, dest_ned_m, is_terrain_alt)) {
348 0 : return false;
349 : }
350 :
351 : // apply destination as the next waypoint leg
352 446 : return set_wp_destination_next_NED_m(dest_ned_m, is_terrain_alt, arc_rad);
353 : }
354 :
355 : // Gets the current waypoint destination as a Location object.
356 : // Altitude frame will be ABOVE_TERRAIN or ABOVE_ORIGIN depending on path configuration.
357 : // Returns false if origin is not set or coordinate conversion fails.
358 60461 : bool AC_WPNav::get_wp_destination_loc(Location& destination) const
359 : {
360 : // retrieve global origin for coordinate conversion
361 60461 : if (!AP::ahrs().get_origin(destination)) {
362 376 : return false;
363 : }
364 :
365 : // convert NED waypoint to global Location format with appropriate altitude frame
366 60085 : destination = Location::from_ekf_offset_NED_m(get_wp_destination_NED_m(), _is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
367 60085 : return true;
368 : }
369 :
370 : // Sets waypoint destination using NEU position vector in centimeters from EKF origin.
371 : // See set_wp_destination_NED_m() for full details.
372 12 : bool AC_WPNav::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool is_terrain_alt)
373 : {
374 12 : Vector3p destination_ned_m = Vector3p(destination_neu_cm.x, destination_neu_cm.y, -destination_neu_cm.z) * 0.01;
375 12 : return set_wp_destination_NED_m(destination_ned_m, is_terrain_alt);
376 : }
377 :
378 : // Sets waypoint destination using NED position vector in meters from EKF origin.
379 : // If `is_terrain_alt` is true, altitude is interpreted as height above terrain.
380 : // Reinitializes the current leg if interrupted, updates origin, and computes trajectory.
381 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path)
382 : // Returns false if terrain offset cannot be determined when required.
383 950 : bool AC_WPNav::set_wp_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_rad)
384 : {
385 : // re-initialise if previous destination has been interrupted
386 950 : if (!is_active() || !_flags.reached_destination) {
387 26 : wp_and_spline_init_m(_wp_desired_speed_ne_ms);
388 : }
389 :
390 950 : _scurve_prev_leg.init();
391 950 : float origin_speed_m = 0.0f;
392 :
393 : // use previous destination as origin
394 950 : _origin_ned_m = _destination_ned_m;
395 :
396 950 : if (is_terrain_alt == _is_terrain_alt) {
397 937 : if (_this_leg_is_spline) {
398 : // Use velocity from end of spline leg to seed new S-curve origin speed
399 5 : Vector3f curr_target_vel_ned_ms = _pos_control.get_vel_desired_NED_ms();
400 5 : curr_target_vel_ned_ms.z -= _pos_control.get_vel_offset_D_ms();
401 5 : origin_speed_m = curr_target_vel_ned_ms.length();
402 : } else {
403 : // Preserve current leg profile to enable blending with new leg
404 932 : _scurve_prev_leg = _scurve_this_leg;
405 : }
406 13 : } else if (!convert_origin_to_alt_frame(is_terrain_alt)) {
407 0 : return false;
408 : }
409 :
410 : // update destination
411 950 : _destination_ned_m = destination_ned_m;
412 950 : _is_terrain_alt = is_terrain_alt;
413 :
414 950 : if (_flags.fast_waypoint && !_this_leg_is_spline && !_next_leg_is_spline && !_scurve_next_leg.finished()) {
415 : // Reuse preloaded next leg if valid to avoid unnecessary recalculation
416 394 : _scurve_this_leg = _scurve_next_leg;
417 : } else {
418 : // Generate a new S-curve segment to the new destination
419 1112 : _scurve_this_leg.calculate_track(_origin_ned_m, _destination_ned_m, arc_rad,
420 556 : _pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
421 : get_wp_acceleration_mss(), get_accel_D_mss(), get_corner_acceleration_mss(),
422 : _scurve_snap_max_mssss, _scurve_jerk_max_msss);
423 556 : if (!is_zero(origin_speed_m)) {
424 : // If we have a valid starting speed, seed it into the S-curve
425 5 : _scurve_this_leg.set_origin_speed_max(origin_speed_m);
426 : }
427 : }
428 :
429 950 : _this_leg_is_spline = false;
430 950 : _this_leg_is_circle = false;
431 950 : _scurve_next_leg.init();
432 950 : _next_destination_ned_m.zero(); // clear next destination_ned_m
433 950 : _flags.fast_waypoint = false; // default waypoint back to slow
434 950 : _flags.reached_destination = false;
435 :
436 950 : return true;
437 : }
438 :
439 : // Sets a circular-orbit destination about center_ne_m using the S-curve engine.
440 : // See the header for the full parameter description. The current destination is used as the
441 : // leg origin and must lie on the circle; the radius is derived from origin-to-center distance.
442 4 : bool AC_WPNav::set_circle_destination_NED_m(const Vector2f& center_ne_m, float turns_signed, float dest_d_m, bool is_terrain_alt)
443 : {
444 : // re-initialise if previous destination has been interrupted
445 4 : if (!is_active() || !_flags.reached_destination) {
446 0 : wp_and_spline_init_m(_wp_desired_speed_ne_ms);
447 : }
448 :
449 4 : _scurve_prev_leg.init();
450 :
451 : // use previous destination as origin; it is assumed to lie on the circle
452 4 : _origin_ned_m = _destination_ned_m;
453 :
454 4 : if ((is_terrain_alt != _is_terrain_alt) && !convert_origin_to_alt_frame(is_terrain_alt)) {
455 0 : return false;
456 : }
457 :
458 : // build the S-curve orbit leg; net climb is applied linearly along the arc
459 4 : const float total_angle_rad = turns_signed * M_2PI;
460 4 : const float climb_d_m = dest_d_m - _origin_ned_m.z;
461 8 : _scurve_this_leg.calculate_circle_track(_origin_ned_m, center_ne_m, total_angle_rad, climb_d_m,
462 4 : _pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
463 : get_wp_acceleration_mss(), get_accel_D_mss(), get_corner_acceleration_mss(),
464 : _scurve_snap_max_mssss, _scurve_jerk_max_msss);
465 :
466 : // the leg itself is the source of truth for where the orbit ends (for whole turns it
467 : // returns to the origin's NE position). A degenerate arc leaves the origin-to-destination
468 : // vector zero, so the destination stays at the origin and the leg completes in place.
469 4 : _destination_ned_m = _origin_ned_m + _scurve_this_leg.get_origin_to_destination().topostype();
470 4 : _is_terrain_alt = is_terrain_alt;
471 :
472 4 : _this_leg_is_spline = false;
473 4 : _this_leg_is_circle = true;
474 4 : _scurve_next_leg.init();
475 4 : _next_destination_ned_m.zero();
476 4 : _flags.fast_waypoint = false;
477 4 : _flags.reached_destination = false;
478 :
479 4 : return true;
480 : }
481 :
482 : // Sets the next waypoint destination using a NED position vector in meters.
483 : // Only updates if terrain frame matches current leg.
484 : // Calculates trajectory preview for smoother transition into next segment.
485 : // Updates velocity handoff if previous leg is a spline.
486 : // arc_rad specifies the signed arc angle in radians for an ARC_WAYPOINT segment (0 for straight path)
487 478 : bool AC_WPNav::set_wp_destination_next_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, float arc_rad)
488 : {
489 : // do not add next point if alt types don't match
490 478 : if (is_terrain_alt != _is_terrain_alt) {
491 0 : return true;
492 : }
493 :
494 : // Preload next S-curve leg with current speed and acceleration constraints
495 956 : _scurve_next_leg.calculate_track(_destination_ned_m, destination_ned_m, arc_rad,
496 478 : _pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
497 : get_wp_acceleration_mss(), get_accel_D_mss(), get_corner_acceleration_mss(),
498 : _scurve_snap_max_mssss, _scurve_jerk_max_msss);
499 478 : if (_this_leg_is_spline) {
500 5 : const float this_leg_dest_speed_max_ms = _spline_this_leg.get_destination_speed_max();
501 : // Pass velocity forward to allow continuous spline-scurve transition
502 5 : const float next_leg_origin_speed_max_ms = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max_ms);
503 5 : _spline_this_leg.set_destination_speed_max(next_leg_origin_speed_max_ms);
504 : }
505 478 : _next_leg_is_spline = false;
506 :
507 : // Enable fast waypoint transition since next leg is known
508 478 : _flags.fast_waypoint = true;
509 :
510 : // Store the upcoming destination for reference
511 478 : _next_destination_ned_m = destination_ned_m;
512 :
513 478 : return true;
514 : }
515 :
516 : // Computes the horizontal stopping point in NE frame, returned in centimeters.
517 : // See get_wp_stopping_point_NE_m() for full details.
518 0 : void AC_WPNav::get_wp_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const
519 : {
520 0 : Vector2p stopping_point_ne_m;
521 0 : get_wp_stopping_point_NE_m(stopping_point_ne_m);
522 0 : stopping_point_ne_cm = stopping_point_ne_m.tofloat() * 100.0;
523 0 : }
524 :
525 : // Computes the horizontal stopping point in NE frame, in meters, based on current velocity and configured acceleration.
526 : // This is the point where the vehicle would come to a stop if decelerated using the configured limits.
527 0 : void AC_WPNav::get_wp_stopping_point_NE_m(Vector2p& stopping_point_ne_m) const
528 : {
529 : // request stopping point from the position controller
530 0 : _pos_control.get_stopping_point_NE_m(stopping_point_ne_m);
531 0 : }
532 :
533 : // Computes the full 3D NEU stopping point vector in centimeters based on current kinematics.
534 : // See get_wp_stopping_point_NED_m() for full details.
535 12 : void AC_WPNav::get_wp_stopping_point_NEU_cm(Vector3f& stopping_point_neu_cm) const
536 : {
537 12 : Vector3p stopping_point_ned_m;
538 : // compute stopping point using meters
539 12 : get_wp_stopping_point_NED_m(stopping_point_ned_m);
540 : // convert result back to centimeters
541 12 : stopping_point_neu_cm = Vector3f(stopping_point_ned_m.x, stopping_point_ned_m.y, -stopping_point_ned_m.z) * 100.0;
542 12 : }
543 :
544 : // Computes the full 3D NED stopping point in meters based on current velocity and configured acceleration in all axes.
545 : // Represents where the vehicle will stop if decelerated from current velocity using configured limits.
546 15140 : void AC_WPNav::get_wp_stopping_point_NED_m(Vector3p& stopping_point_ned_m) const
547 : {
548 : // get horizontal stopping point (North and East)
549 15140 : _pos_control.get_stopping_point_NE_m(stopping_point_ned_m.xy());
550 : // get vertical stopping point (Down)
551 15140 : _pos_control.get_stopping_point_D_m(stopping_point_ned_m.z);
552 15140 : }
553 :
554 : // Advances the target location along the current path segment.
555 : // Updates target position, velocity, and acceleration based on jerk-limited profile (or spline).
556 : // Returns true if the update succeeded (e.g., terrain data was available).
557 4010709 : bool AC_WPNav::advance_wp_target_along_track(float dt)
558 : {
559 : // calculate terrain offset if using alt-above-terrain frame
560 4010709 : float terr_offset_d_m = 0.0f;
561 4010709 : if (_is_terrain_alt && !get_terrain_D_m(terr_offset_d_m)) {
562 1660 : return false;
563 : }
564 :
565 : // calculate terrain-based velocity scaling factor
566 4009049 : const float offset_d_scalar = _pos_control.terrain_scaler_D_m(terr_offset_d_m, get_terrain_margin_m());
567 :
568 : // input shape the terrain offset
569 4009049 : _pos_control.set_pos_terrain_target_D_m(terr_offset_d_m);
570 :
571 : // get position controller's post-shaped position offset for use in position error computation
572 4009049 : const Vector3p& psc_pos_offset_ned_m = _pos_control.get_pos_offset_NED_m();
573 :
574 : // compute current position in NED frame, adjusted to destination frame (e.g., terrain-relative if needed)
575 4009049 : Vector3p curr_pos_ned_m = _pos_control.get_pos_estimate_NED_m() - psc_pos_offset_ned_m;
576 4009049 : curr_pos_ned_m.z -= terr_offset_d_m;
577 :
578 : // get desired velocity and remove offset
579 4009049 : Vector3f curr_target_vel_ned_ms = _pos_control.get_vel_desired_NED_ms();
580 4009049 : curr_target_vel_ned_ms.z -= _pos_control.get_vel_offset_D_ms();
581 :
582 : // scale progression time (track_dt_scalar) based on aircraft’s speed alignment with desired path
583 4009049 : float track_dt_scalar = 1.0f;
584 4009049 : if (is_positive(curr_target_vel_ned_ms.length_squared())) {
585 3294620 : Vector3f track_direction_neu = curr_target_vel_ned_ms.normalized();
586 3294620 : const float track_error_ned_m = _pos_control.get_pos_error_NED_m().dot(track_direction_neu);
587 3294620 : const float track_velocity_ned_ms = _pos_control.get_vel_estimate_NED_ms().dot(track_direction_neu);
588 : // limit time step scalar to [0,1], with 5% buffer
589 3294620 : track_dt_scalar = constrain_float(0.05f + (track_velocity_ned_ms - _pos_control.NE_get_pos_p().kP() * track_error_ned_m) / curr_target_vel_ned_ms.length(), 0.0f, 1.0f);
590 : }
591 :
592 : // compute velocity scaling (vel_dt_scalar) and apply jerk-limited velocity shaping
593 4009049 : float vel_dt_scalar = 1.0;
594 4009049 : if (is_positive(_wp_desired_speed_ne_ms)) {
595 4009049 : update_vel_accel(_offset_vel_ms, _offset_accel_mss, dt, 0.0, 0.0);
596 4009049 : const float vel_input_ms = !_paused ? _wp_desired_speed_ne_ms * offset_d_scalar : 0.0;
597 4009049 : shape_vel_accel(vel_input_ms, 0.0, _offset_vel_ms, _offset_accel_mss, -get_wp_acceleration_mss(), get_wp_acceleration_mss(),
598 4009049 : _pos_control.get_shaping_jerk_NE_msss(), dt, true);
599 4009049 : vel_dt_scalar = _offset_vel_ms / _wp_desired_speed_ne_ms;
600 : }
601 :
602 : // apply exponential filter to track_dt_scalar using jerk-based time constant
603 4009049 : float track_dt_scalar_tc = 1.0f;
604 4009049 : if (!is_zero(_wp_jerk_msss)) {
605 4009049 : track_dt_scalar_tc = get_wp_acceleration_mss()/_wp_jerk_msss;
606 : }
607 4009049 : _track_dt_scalar += (track_dt_scalar - _track_dt_scalar) * (dt / track_dt_scalar_tc);
608 :
609 : // select path generator (S-curve or spline) and compute target state
610 4009049 : Vector3p target_pos_ned_m;
611 4009049 : Vector3f target_vel_ned_ms, target_accel_ned_mss;
612 : bool s_finished;
613 4009049 : if (!_this_leg_is_spline) {
614 : // update target position, velocity and acceleration
615 3901332 : target_pos_ned_m = _origin_ned_m;
616 3901332 : s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_m, get_corner_acceleration_mss(), _flags.fast_waypoint, _track_dt_scalar * vel_dt_scalar * dt, target_pos_ned_m, target_vel_ned_ms, target_accel_ned_mss);
617 : } else {
618 : // splinetarget_vel
619 107717 : target_vel_ned_ms = curr_target_vel_ned_ms;
620 107717 : _spline_this_leg.advance_target_along_track(_track_dt_scalar * vel_dt_scalar * dt, target_pos_ned_m, target_vel_ned_ms);
621 107717 : s_finished = _spline_this_leg.reached_destination();
622 : }
623 :
624 : // apply scaled acceleration offset along current direction of travel
625 4009049 : Vector3f accel_offset_ned_mss;
626 4009049 : if (is_positive(target_vel_ned_ms.length_squared())) {
627 3296409 : Vector3f track_direction_neu = target_vel_ned_ms.normalized();
628 3296409 : accel_offset_ned_mss = track_direction_neu * _offset_accel_mss * target_vel_ned_ms.length() / _wp_desired_speed_ne_ms;
629 : }
630 :
631 4009049 : target_vel_ned_ms *= vel_dt_scalar;
632 4009049 : target_accel_ned_mss *= sq(vel_dt_scalar);
633 4009049 : target_accel_ned_mss += accel_offset_ned_mss;
634 :
635 : // send updated position, velocity, and acceleration targets to position controller
636 4009049 : _pos_control.set_pos_vel_accel_NED_m(target_pos_ned_m, target_vel_ned_ms, target_accel_ned_mss);
637 :
638 : // check if waypoint has been reached based on mode and radius
639 4009049 : if (!_flags.reached_destination) {
640 3329592 : if (s_finished) {
641 : // "fast" waypoints are complete once the intermediate point reaches the destination
642 1247 : if (_flags.fast_waypoint) {
643 445 : _flags.reached_destination = true;
644 : } else {
645 : // regular waypoints also require the copter to be within the waypoint radius
646 802 : const Vector3f dist_to_dest_m = (curr_pos_ned_m - _destination_ned_m).tofloat();
647 802 : if (dist_to_dest_m.length_squared() <= sq(_wp_radius_m)) {
648 399 : _flags.reached_destination = true;
649 : }
650 : }
651 : }
652 : }
653 :
654 : // successfully advanced along track
655 4009049 : return true;
656 : }
657 :
658 : // Updates the current and next path segment to reflect new speed and acceleration limits.
659 : // Should be called after modifying NE/U controller limits or vehicle configuration.
660 686 : void AC_WPNav::update_track_with_speed_accel_limits()
661 : {
662 : // update speed and acceleration limits for the current segment
663 686 : if (_this_leg_is_spline) {
664 0 : _spline_this_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
665 : get_wp_acceleration_mss(), get_accel_D_mss());
666 : } else {
667 686 : _scurve_this_leg.set_speed_max(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms());
668 : }
669 :
670 : // update speed and acceleration limits for the next segment
671 686 : if (_next_leg_is_spline) {
672 0 : _spline_next_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
673 : get_wp_acceleration_mss(), get_accel_D_mss());
674 : } else {
675 686 : _scurve_next_leg.set_speed_max(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms());
676 : }
677 686 : }
678 :
679 : // Returns the horizontal distance to the destination waypoint in centimeters.
680 : // See get_wp_distance_to_destination_m() for full details.
681 57479 : float AC_WPNav::get_wp_distance_to_destination_cm() const
682 : {
683 : // convert distance from meters to centimeters
684 57479 : return get_wp_distance_to_destination_m() * 100.0;
685 : }
686 :
687 : // Returns the horizontal distance in meters between the current position and the destination waypoint.
688 190474 : float AC_WPNav::get_wp_distance_to_destination_m() const
689 : {
690 : // calculate 2D ground distance between current position and destination in NE frame
691 190474 : return get_horizontal_distance(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat());
692 : }
693 :
694 : // Returns the bearing to the current waypoint destination in centidegrees.
695 : // See get_wp_bearing_to_destination_rad() for full details.
696 61213 : int32_t AC_WPNav::get_wp_bearing_to_destination_cd() const
697 : {
698 : // compute heading from current position to destination in centidegrees
699 61213 : return get_bearing_cd(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat());
700 : }
701 :
702 : // Returns the bearing to the current waypoint destination in radians.
703 : // The bearing is measured clockwise from North, with 0 = North.
704 126811 : float AC_WPNav::get_wp_bearing_to_destination_rad() const
705 : {
706 : // compute heading from current position to destination in radians
707 126811 : return get_bearing_rad(_pos_control.get_pos_estimate_NED_m().xy().tofloat(), _destination_ned_m.xy().tofloat());
708 : }
709 :
710 : // Runs the waypoint navigation controller.
711 : // Advances the target position and updates the position controller.
712 : // Should be called at 100 Hz or higher for accurate tracking.
713 4010709 : bool AC_WPNav::update_wpnav()
714 : {
715 : // check for changes in WPNAV_SPEED parameter (horizontal speed target)
716 4010709 : if (_check_wp_speed_change) {
717 3708381 : if (!is_equal(_wp_speed_ms.get(), _last_wp_speed_ms)) {
718 : // apply new WPNAV_SPEED value
719 23 : set_speed_NE_ms(get_default_speed_NE_ms());
720 23 : _last_wp_speed_ms = _wp_speed_ms;
721 : }
722 : }
723 : // check for climb and descent speed updates
724 4010709 : if (!is_equal(_wp_speed_up_ms.get(), _last_wp_speed_up_ms)) {
725 11 : set_speed_up_ms(get_default_speed_up_ms());
726 11 : _last_wp_speed_up_ms = _wp_speed_up_ms;
727 : }
728 4010709 : if (!is_equal(_wp_speed_down_ms.get(), _last_wp_speed_down_ms)) {
729 8 : set_speed_down_ms(get_default_speed_down_ms());
730 8 : _last_wp_speed_down_ms = _wp_speed_down_ms;
731 : }
732 :
733 : // advance the waypoint target based on current position and timing
734 4010709 : bool ret = true;
735 4010709 : if (!advance_wp_target_along_track(_pos_control.get_dt_s())) {
736 : // To-Do: handle inability to advance along track (probably because of missing terrain data)
737 1660 : ret = false;
738 : }
739 :
740 : // run the horizontal position controller
741 4010709 : _pos_control.NE_update_controller();
742 :
743 : // record update time for is_active()
744 4010709 : _wp_last_update_ms = AP_HAL::millis();
745 :
746 4010709 : return ret;
747 : }
748 :
749 : // Returns true if update_wpnav() has been called within the last 200 ms.
750 : // Used to check if waypoint navigation is currently active.
751 51660 : bool AC_WPNav::is_active() const
752 : {
753 : // consider WPNAV active if last update was within 200 milliseconds
754 51660 : return (AP_HAL::millis() - _wp_last_update_ms) < 200;
755 : }
756 :
757 : // Forces a stop at the next waypoint instead of continuing to the subsequent one.
758 : // Used by Dijkstra’s object avoidance when the future path is obstructed.
759 : // Only affects regular (non-spline) waypoints.
760 : // Returns true if the stop behavior was newly enforced.
761 0 : bool AC_WPNav::force_stop_at_next_wp()
762 : {
763 : // skip if vehicle was already going to stop at the next waypoint
764 0 : if (!_flags.fast_waypoint) {
765 0 : return false;
766 : }
767 :
768 0 : _flags.fast_waypoint = false;
769 :
770 : // override final velocity for current leg and reset preview for next leg
771 0 : if (!_this_leg_is_spline) {
772 0 : _scurve_this_leg.set_destination_speed_max(0);
773 : }
774 0 : if (!_next_leg_is_spline) {
775 0 : _scurve_next_leg.init();
776 : }
777 :
778 0 : return true;
779 : }
780 :
781 : // Returns terrain offset in meters above the EKF origin at the current position.
782 : // Positive values mean terrain lies above the EKF origin altitude.
783 : // Source may be rangefinder or terrain database depending on availability.
784 246547 : bool AC_WPNav::get_terrain_U_m(float& terrain_u_m)
785 : {
786 : // determine terrain data source and compute offset accordingly
787 246547 : switch (get_terrain_source()) {
788 0 : case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE:
789 0 : return false;
790 :
791 189740 : case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
792 189740 : if (_rangefinder_healthy) {
793 : // return previously saved offset based on rangefinder
794 188080 : terrain_u_m = _rangefinder_terrain_u_m;
795 188080 : return true;
796 : }
797 1660 : return false;
798 :
799 56807 : case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
800 : #if AP_TERRAIN_AVAILABLE
801 56807 : float height_above_terrain_m = 0.0f;
802 56807 : AP_Terrain *terrain = AP::terrain();
803 113614 : if (terrain != nullptr &&
804 56807 : terrain->height_above_terrain(height_above_terrain_m, true)) {
805 : // compute offset as difference between current altitude and terrain height
806 56807 : terrain_u_m = _pos_control.get_pos_estimate_U_m() - height_above_terrain_m;
807 56807 : return true;
808 : }
809 : #endif
810 0 : return false;
811 : }
812 :
813 : // unreachable fallback path
814 0 : return false;
815 : }
816 : // Converts _origin_ned_m.z between the terrain-relative and origin-relative altitude frames and
817 : // re-seeds the position controller's terrain offset to match. Call only when the new leg's
818 : // frame differs from the current one.
819 : // Returns false if terrain data is required but unavailable.
820 13 : bool AC_WPNav::convert_origin_to_alt_frame(bool is_terrain_alt)
821 : {
822 : float terrain_d_m;
823 13 : if (!get_terrain_D_m(terrain_d_m)) {
824 0 : return false;
825 : }
826 :
827 13 : if (is_terrain_alt) {
828 : // Convert origin.z to terrain-relative altitude
829 13 : _origin_ned_m.z -= terrain_d_m;
830 13 : _pos_control.init_pos_terrain_D_m(terrain_d_m);
831 : } else {
832 : // Convert origin.z to origin-relative altitude
833 0 : _origin_ned_m.z += terrain_d_m;
834 0 : _pos_control.init_pos_terrain_D_m(0.0);
835 : }
836 :
837 13 : return true;
838 : }
839 :
840 211829 : bool AC_WPNav::get_terrain_D_m(float& terrain_d_m)
841 : {
842 211829 : if (!get_terrain_U_m(terrain_d_m)) {
843 1660 : return false;
844 : }
845 210169 : terrain_d_m *= -1.0;
846 210169 : return true;
847 : }
848 :
849 : ///
850 : /// spline methods
851 : ///
852 :
853 : // Sets the current spline waypoint using global coordinates.
854 : // Converts `destination` and `next_destination` to NED position vectors and sets up a spline between them.
855 : // Returns false if conversion from location to vector fails.
856 24 : bool AC_WPNav::set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline)
857 : {
858 : // convert destination location to NED vector and altitude frame
859 24 : Vector3p dest_ned_m;
860 : bool dest_is_terrain_alt;
861 24 : if (!get_vector_NED_m(destination, dest_ned_m, dest_is_terrain_alt)) {
862 0 : return false;
863 : }
864 :
865 : // convert next destination location to NED vector and altitude frame
866 24 : Vector3p next_dest_ned_m;
867 : bool next_dest_is_terrain_alt;
868 24 : if (!get_vector_NED_m(next_destination, next_dest_ned_m, next_dest_is_terrain_alt)) {
869 0 : return false;
870 : }
871 :
872 : // initialize spline path between converted destination and next_destination
873 24 : return set_spline_destination_NED_m(dest_ned_m, dest_is_terrain_alt, next_dest_ned_m, next_dest_is_terrain_alt, next_is_spline);
874 : }
875 :
876 : // Sets the next spline segment using global coordinates.
877 : // Converts the next and next-next destinations to NED position vectors and initializes the spline transition.
878 : // Returns false if any conversion from location to vector fails.
879 14 : bool AC_WPNav::set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline)
880 : {
881 : // convert next_destination location to NED vector and determine if it uses terrain-relative altitude
882 14 : Vector3p next_dest_ned_m;
883 : bool next_dest_is_terrain_alt;
884 14 : if (!get_vector_NED_m(next_destination, next_dest_ned_m, next_dest_is_terrain_alt)) {
885 0 : return false;
886 : }
887 :
888 : // convert next_next_destination location to NED vector and determine if it uses terrain-relative altitude
889 14 : Vector3p next_next_dest_ned_m;
890 : bool next_next_dest_is_terr_alt;
891 14 : if (!get_vector_NED_m(next_next_destination, next_next_dest_ned_m, next_next_dest_is_terr_alt)) {
892 0 : return false;
893 : }
894 :
895 : // initialize next spline segment using converted waypoints
896 14 : return set_spline_destination_next_NED_m(next_dest_ned_m, next_dest_is_terrain_alt, next_next_dest_ned_m, next_next_dest_is_terr_alt, next_next_is_spline);
897 : }
898 :
899 : // Sets the current spline waypoint using NED position vectors in meters.
900 : // Initializes a spline path from `destination_ned_m` to `next_destination_ned_m`, respecting terrain altitude framing.
901 : // Both waypoints must use the same altitude frame (either above terrain or above origin).
902 : // Returns false if terrain altitude cannot be determined when required.
903 24 : bool AC_WPNav::set_spline_destination_NED_m(const Vector3p& destination_ned_m, bool is_terrain_alt, const Vector3p& next_destination_ned_m, bool next_is_terrain_alt, bool next_is_spline)
904 : {
905 : // re-initialise path state if previous destination was not completed or controller inactive
906 24 : if (!is_active() || !_flags.reached_destination) {
907 2 : wp_and_spline_init_m(_wp_desired_speed_ne_ms);
908 : }
909 :
910 : // update spline calculators speeds and accelerations
911 24 : _spline_this_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
912 24 : _pos_control.NE_get_max_accel_mss(), _pos_control.D_get_max_accel_mss());
913 :
914 : // calculate origin and origin velocity vector
915 24 : Vector3f origin_vector_ned_m;
916 24 : if (is_terrain_alt == _is_terrain_alt) {
917 24 : if (_flags.fast_waypoint) {
918 : // calculate origin vector
919 14 : if (_this_leg_is_spline) {
920 : // use velocity vector from end of previous spline segment
921 12 : origin_vector_ned_m = _spline_this_leg.get_destination_vel();
922 : } else {
923 : // use direction vector from previous straight-line segment
924 2 : origin_vector_ned_m = (_destination_ned_m - _origin_ned_m).tofloat();
925 : }
926 : }
927 :
928 : // use previous destination as origin
929 24 : _origin_ned_m = _destination_ned_m;
930 : } else {
931 :
932 : // use previous destination as origin
933 0 : _origin_ned_m = _destination_ned_m;
934 :
935 : // get current alt above terrain
936 : float terrain_d_m;
937 0 : if (!get_terrain_D_m(terrain_d_m)) {
938 0 : return false;
939 : }
940 :
941 : // convert origin to alt-above-terrain if necessary
942 0 : if (is_terrain_alt) {
943 : // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
944 0 : _origin_ned_m.z -= terrain_d_m;
945 0 : _pos_control.init_pos_terrain_D_m(terrain_d_m);
946 : } else {
947 : // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
948 0 : _origin_ned_m.z += terrain_d_m;
949 0 : _pos_control.init_pos_terrain_D_m(0.0);
950 : }
951 : }
952 :
953 : // store destination location
954 24 : _destination_ned_m = destination_ned_m;
955 24 : _is_terrain_alt = is_terrain_alt;
956 :
957 : // calculate destination velocity vector
958 24 : Vector3f destination_vector_ned_m;
959 24 : if (is_terrain_alt == next_is_terrain_alt) {
960 24 : if (next_is_spline) {
961 : // aim to leave segment in direction of full arc (origin to next)
962 12 : destination_vector_ned_m = (next_destination_ned_m - _origin_ned_m).tofloat();
963 : } else {
964 : // aim to leave segment in direction of following leg
965 12 : destination_vector_ned_m = (next_destination_ned_m - _destination_ned_m).tofloat();
966 : }
967 : }
968 24 : _flags.fast_waypoint = !destination_vector_ned_m.is_zero();
969 :
970 : // setup spline leg using origin and destination vectors
971 24 : _spline_this_leg.set_origin_and_destination(_origin_ned_m, _destination_ned_m, origin_vector_ned_m, destination_vector_ned_m);
972 24 : _this_leg_is_spline = true;
973 24 : _this_leg_is_circle = false;
974 24 : _flags.reached_destination = false;
975 :
976 24 : return true;
977 : }
978 :
979 : // Sets the next spline segment using NED position vectors in meters.
980 : // Creates a spline path from the current destination to `next_destination_ned_m`, and prepares transition toward `next_next_destination_ned_m`.
981 : // All waypoints must use the same altitude frame (above terrain or origin).
982 : // Returns false if terrain data is missing and required.
983 14 : bool AC_WPNav::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)
984 : {
985 : // do not add next point if alt types don't match
986 14 : if (next_is_terrain_alt != _is_terrain_alt) {
987 0 : return true;
988 : }
989 :
990 : // calculate origin and origin velocity vector
991 14 : Vector3f origin_vector_ned_m;
992 14 : if (_this_leg_is_spline) {
993 : // use final velocity vector from current spline segment
994 12 : origin_vector_ned_m = _spline_this_leg.get_destination_vel();
995 : } else {
996 : // use vector from previous origin to current destination
997 2 : origin_vector_ned_m = (_destination_ned_m - _origin_ned_m).tofloat();
998 : }
999 :
1000 : // calculate destination velocity vector
1001 14 : Vector3f destination_vector_ned_m;
1002 14 : if (next_is_terrain_alt == next_next_is_terrain_alt) {
1003 14 : if (next_next_is_spline) {
1004 : // aim to leave segment in direction of the arc from current to next-next
1005 11 : destination_vector_ned_m = (next_next_destination_ned_m - _destination_ned_m).tofloat();
1006 : } else {
1007 : // aim to leave segment in direction of the upcoming straight leg
1008 3 : destination_vector_ned_m = (next_next_destination_ned_m - next_destination_ned_m).tofloat();
1009 : }
1010 : }
1011 :
1012 : // update spline calculators speeds and accelerations
1013 14 : _spline_next_leg.set_speed_accel(_pos_control.NE_get_max_speed_ms(), _pos_control.get_max_speed_up_ms(), _pos_control.get_max_speed_down_ms(),
1014 14 : _pos_control.NE_get_max_accel_mss(), _pos_control.D_get_max_accel_mss());
1015 :
1016 : // setup next spline leg. Note this could be made local
1017 14 : _spline_next_leg.set_origin_and_destination(_destination_ned_m, next_destination_ned_m, origin_vector_ned_m, destination_vector_ned_m);
1018 14 : _next_leg_is_spline = true;
1019 :
1020 : // next destination provided so fast waypoint
1021 14 : _flags.fast_waypoint = true;
1022 :
1023 : // update this_leg's final velocity to match next spline leg
1024 14 : if (!_this_leg_is_spline) {
1025 2 : _scurve_this_leg.set_destination_speed_max(_spline_next_leg.get_origin_speed_max());
1026 : } else {
1027 12 : _spline_this_leg.set_destination_speed_max(_spline_next_leg.get_origin_speed_max());
1028 : }
1029 :
1030 14 : return true;
1031 : }
1032 :
1033 : // Converts a Location to a NED position vector in meters from the EKF origin.
1034 : // Sets `is_terrain_alt` to true if the resulting Z position is relative to terrain.
1035 : // Returns false if terrain data is unavailable or conversion fails.
1036 1793 : bool AC_WPNav::get_vector_NED_m(const Location &loc, Vector3p &pos_from_origin_ned_m, bool &is_terrain_alt)
1037 : {
1038 : // convert horizontal position (latitude/longitude) to NE vector from EKF origin
1039 1793 : Vector2p loc_pos_from_origin_ned_m;
1040 1793 : if (!loc.get_vector_xy_from_origin_NE_m(loc_pos_from_origin_ned_m)) {
1041 8 : return false;
1042 : }
1043 :
1044 : // convert altitude
1045 1785 : if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
1046 : float terrain_u_m;
1047 109 : if (!loc.get_alt_m(Location::AltFrame::ABOVE_TERRAIN, terrain_u_m)) {
1048 0 : return false;
1049 : }
1050 109 : pos_from_origin_ned_m.z = -terrain_u_m;
1051 109 : is_terrain_alt = true;
1052 : } else {
1053 1676 : is_terrain_alt = false;
1054 : float origin_alt_m;
1055 1676 : if (!loc.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, origin_alt_m)) {
1056 0 : return false;
1057 : }
1058 1676 : pos_from_origin_ned_m.z = -origin_alt_m;
1059 1676 : is_terrain_alt = false;
1060 : }
1061 :
1062 : // set horizontal components (x/y) of NED vector after successful conversion
1063 1785 : pos_from_origin_ned_m.xy() = loc_pos_from_origin_ned_m;
1064 :
1065 1785 : return true;
1066 : }
1067 :
1068 : // Calculates s-curve jerk and snap limits based on attitude controller capabilities.
1069 : // Updates _scurve_jerk_max_msss and _scurve_snap_max_mssss with constrained values.
1070 15182 : void AC_WPNav::calc_scurve_jerk_and_snap()
1071 : {
1072 : // calculate max horizontal jerk based on roll/pitch angular velocity limits
1073 15182 : _scurve_jerk_max_msss = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS);
1074 15182 : if (is_zero(_scurve_jerk_max_msss)) {
1075 14922 : _scurve_jerk_max_msss = _wp_jerk_msss;
1076 : } else {
1077 260 : _scurve_jerk_max_msss = MIN(_scurve_jerk_max_msss, _wp_jerk_msss);
1078 : }
1079 :
1080 : // calculate maximum snap (rate of change of jerk)
1081 : // uses input time constant as proxy for lean angle actuation latency
1082 15182 : _scurve_snap_max_mssss = (_scurve_jerk_max_msss * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f));
1083 :
1084 : // constrain snap based on roll/pitch angular acceleration capability
1085 15182 : const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS;
1086 15182 : if (is_positive(snap)) {
1087 15182 : _scurve_snap_max_mssss = MIN(_scurve_snap_max_mssss, snap);
1088 : }
1089 :
1090 : // apply safety margin: reduce snap to 50% of calculated maximum
1091 15182 : _scurve_snap_max_mssss *= 0.5;
1092 15182 : }
|
