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 <AC_PID/AC_P.h> // P library
7 : #include <AC_PID/AC_PID.h> // PID library
8 : #include <AC_PID/AC_P_1D.h> // P library (1-axis)
9 : #include <AC_PID/AC_P_2D.h> // P library (2-axis)
10 : #include <AC_PID/AC_PI_2D.h> // PI library (2-axis)
11 : #include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
12 : #include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
13 : #include <AP_Scripting/AP_Scripting_config.h>
14 : #include "AC_AttitudeControl.h" // Attitude control library
15 :
16 : #include <AP_Logger/LogStructure.h>
17 :
18 : // position controller default definitions
19 : #define POSCONTROL_ACCEL_NE_MSS 1.0f // default horizontal acceleration in m/s². This is overwritten by waypoint and loiter controllers
20 : #define POSCONTROL_JERK_NE_MSSS 5.0f // default horizontal jerk m/s³
21 :
22 : #define POSCONTROL_STOPPING_DIST_UP_MAX_M 3.0f // max stopping distance (in m) vertically while climbing
23 : #define POSCONTROL_STOPPING_DIST_DOWN_MAX_M 2.0f // max stopping distance (in m) vertically while descending
24 :
25 : #define POSCONTROL_SPEED_MS 5.0f // default horizontal speed in m/s
26 : #define POSCONTROL_SPEED_DOWN_MS 1.5f // default descent rate in m/s
27 : #define POSCONTROL_SPEED_UP_MS 2.5f // default climb rate in m/s
28 :
29 : #define POSCONTROL_ACCEL_D_MSS 2.5f // default vertical acceleration in m/s²
30 : #define POSCONTROL_JERK_D_MSSS 5.0f // default vertical jerk m/s³
31 :
32 : #define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz)
33 :
34 : #define POSCONTROL_OVERSPEED_GAIN_U 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
35 :
36 : #define POSCONTROL_RELAX_TC 0.16f // This is used to decay the I term to 5% in half a second
37 :
38 : class AC_PosControl
39 : {
40 : public:
41 :
42 : /// Constructor
43 : AC_PosControl(AP_AHRS_View& ahrs, const class AP_Motors& motors, AC_AttitudeControl& attitude_control);
44 :
45 : // do not allow copying
46 : CLASS_NO_COPY(AC_PosControl);
47 :
48 : // Sets the timestep between controller updates (seconds).
49 : // This should match the IMU sample time used by the controller.
50 35255076 : void set_dt_s(float dt) { _dt_s = dt; }
51 :
52 : // Returns the timestep used in the controller update (seconds).
53 7277645 : float get_dt_s() const { return _dt_s; }
54 :
55 : // Updates internal NED position and velocity estimates from AHRS.
56 : // Falls back to vertical-only data if horizontal velocity or position is invalid or vibration forces it.
57 : // When high_vibes is true, forces use of vertical fallback for velocity.
58 : void update_estimates(bool high_vibes = false);
59 :
60 : // Returns the jerk limit for horizontal path shaping in m/s³.
61 : // Used to constrain acceleration changes in trajectory generation.
62 4009049 : float get_shaping_jerk_NE_msss() const { return _shaping_jerk_ne_msss; }
63 :
64 :
65 : ///
66 : /// 3D position shaper
67 : ///
68 :
69 : // Sets a new NED position target in meters and computes a jerk-limited trajectory.
70 : // Updates internal acceleration commands using a smooth kinematic path constrained
71 : // by configured acceleration and jerk limits.
72 : // The path can be offset vertically to follow the terrain by providing the current
73 : // terrain level in the NED frame and the terrain margin. Terrain margin is used to
74 : // constrain horizontal velocity to avoid vertical buffer violation.
75 : void input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m);
76 :
77 : // Returns a scaling factor for horizontal velocity in m/s to ensure
78 : // the vertical controller maintains a safe distance above terrain.
79 : float terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const;
80 :
81 : ///
82 : /// Lateral position controller
83 : ///
84 :
85 : // Sets maximum horizontal speed (cm/s) and acceleration (cm/s²) for NE-axis shaping.
86 : // Can be called anytime; transitions are handled smoothly.
87 : // All arguments should be positive.
88 : // See NE_set_max_speed_accel_m() for full details.
89 : void NE_set_max_speed_accel_cm(float speed_cms, float accel_cmss);
90 :
91 : // Sets maximum horizontal speed (m/s) and acceleration (m/s²) for NE-axis shaping.
92 : // These values constrain the kinematic trajectory used by the lateral controller.
93 : // All arguments should be positive.
94 : void NE_set_max_speed_accel_m(float speed_ms, float accel_mss);
95 :
96 : // Sets horizontal correction limits for velocity (cm/s) and acceleration (cm/s²).
97 : // Should be called only during initialization to avoid control discontinuities.
98 : // All arguments should be positive.
99 : // See NE_set_correction_speed_accel_m() for full details.
100 : void NE_set_correction_speed_accel_cm(float speed_cms, float accel_cmss);
101 :
102 : // Sets horizontal correction limits for velocity (m/s) and acceleration (m/s²).
103 : // These values constrain the PID correction path, not the desired trajectory.
104 : // All arguments should be positive.
105 : void NE_set_correction_speed_accel_m(float speed_ms, float accel_mss);
106 :
107 : // Returns maximum horizontal speed in cm/s.
108 : // See NE_get_max_speed_ms() for full details.
109 398817 : float NE_get_max_speed_cms() const { return NE_get_max_speed_ms() * 100.0; }
110 :
111 : // Returns maximum horizontal speed in m/s used for shaping the trajectory.
112 641374 : float NE_get_max_speed_ms() const { return _vel_max_ne_ms; }
113 :
114 : // Returns maximum horizontal acceleration in m/s² used for trajectory shaping.
115 733180 : float NE_get_max_accel_mss() const { return _accel_max_ne_mss; }
116 :
117 : // Sets maximum allowed horizontal position error in meters.
118 : // Used to constrain the output of the horizontal position P controller.
119 1380446 : void NE_set_pos_error_max_m(float error_max_m) { _p_pos_ne_m.set_error_max(error_max_m); }
120 :
121 : // Returns maximum allowed horizontal position error in meters.
122 : float NE_get_pos_error_max_m() const { return _p_pos_ne_m.get_error_max(); }
123 :
124 : // Initializes NE controller to a stationary stopping point with zero velocity and acceleration.
125 : // Use when the expected trajectory begins at rest but the starting position is unspecified.
126 : // The starting position can be retrieved with get_pos_target_NED_m().
127 : void NE_init_controller_stopping_point();
128 :
129 : // Smoothly decays NE acceleration over time to zero while maintaining current velocity and position.
130 : // Reduces output acceleration by ~95% over 0.5 seconds to avoid abrupt transitions.
131 : void NE_relax_velocity_controller();
132 :
133 : // Softens NE controller for landing by reducing position error and suppressing I-term windup.
134 : // Used to make descent behavior more stable near ground contact.
135 : void NE_soften_for_landing();
136 :
137 : // Fully initializes the NE controller with current position, velocity, acceleration, and attitude.
138 : // Intended for normal startup when the full state is known.
139 : // Private function shared by other NE initializers.
140 : void NE_init_controller();
141 :
142 : // Sets the desired NE-plane acceleration in m/s² using jerk-limited shaping.
143 : // Smoothly transitions to the specified acceleration from current kinematic state.
144 : // Constraints: max acceleration and jerk set via NE_set_max_speed_accel_m().
145 : void input_accel_NE_m(const Vector2f& accel_ne_msss);
146 :
147 : // Sets desired NE-plane velocity and acceleration (cm/s, cm/s²) using jerk-limited shaping.
148 : // See input_vel_accel_NE_m() for full details.
149 : void input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true);
150 :
151 : // Sets desired NE-plane velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
152 : // Calculates target acceleration using current kinematics constrained by acceleration and jerk limits.
153 : // If `limit_output` is true, applies limits to total command (desired + correction).
154 : void input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true);
155 :
156 : // Sets desired NE position, velocity, and acceleration (cm, cm/s, cm/s²) with jerk-limited shaping.
157 : // See input_pos_vel_accel_NE_m() for full details.
158 : void input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true);
159 :
160 : // Sets desired NE position, velocity, and acceleration (m, m/s, m/s²) with jerk-limited shaping.
161 : // Calculates acceleration trajectory based on current kinematics and constraints.
162 : // If `limit_output` is true, limits apply to full command (desired + correction).
163 : void input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true);
164 :
165 : // Returns true if the NE position controller has run in the last 5 control loop cycles.
166 : bool NE_is_active() const;
167 :
168 : // Disables NE position correction by setting the target position to the current position.
169 : // Useful to freeze positional control without disrupting velocity control.
170 : void NE_stop_pos_stabilisation();
171 :
172 : // Disables NE position and velocity correction by setting target values to current state.
173 : // Useful to prevent further corrections and freeze motion stabilization in NE axes.
174 : void NE_stop_vel_stabilisation();
175 :
176 : // Applies a scalar multiplier to the NE control loop.
177 : // Set to 0 to disable lateral control; 1 for full authority.
178 0 : void NE_set_control_scale_factor(float ne_control_scale_factor) {
179 0 : _ne_control_scale_factor = ne_control_scale_factor;
180 0 : }
181 :
182 : // Runs the NE-axis position controller, computing output acceleration from position and velocity errors.
183 : // Uses P and PID controllers to generate corrections which are added to feedforward velocity/acceleration.
184 : // Requires all desired targets to be pre-set using the input_* or set_* methods.
185 : void NE_update_controller();
186 :
187 : ///
188 : /// Vertical position controller
189 : ///
190 :
191 : // Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis.
192 : // Descent rate may be positive or negative and is always interpreted as a descent.
193 : // See D_set_max_speed_accel_m() for full details.
194 : // All values must be positive.
195 : void D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss);
196 :
197 : // Sets maximum climb/descent rate (m/s) and vertical acceleration (m/s²) for the U-axis.
198 : // These values are used for jerk-limited kinematic shaping of the vertical trajectory.
199 : // All values must be positive.
200 : void D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss);
201 :
202 : // Sets vertical correction velocity and acceleration limits (cm/s, cm/s²).
203 : // Should only be called during initialization to avoid discontinuities.
204 : // See D_set_correction_speed_accel_m() for full details.
205 : // All values must be positive.
206 : void D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss);
207 :
208 : // Sets vertical correction velocity and acceleration limits (m/s, m/s²).
209 : // These values constrain the correction output of the PID controller.
210 : // All values must be positive.
211 : void D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss);
212 :
213 : // Returns maximum vertical acceleration in m/s² used for shaping the climb/descent trajectory.
214 118448791 : float D_get_max_accel_mss() const { return _accel_max_d_mss; }
215 :
216 : // Returns maximum allowed positive (upward) position error in meters.
217 : float get_pos_error_up_m() const { return _p_pos_d_m.get_error_min(); }
218 :
219 : // Returns maximum allowed negative (downward) position error in meters.
220 : float get_pos_error_down_m() const { return _p_pos_d_m.get_error_max(); }
221 :
222 : // Returns maximum climb rate in cm/s.
223 : // See get_max_speed_up_ms() for full details.
224 3 : float get_max_speed_up_cms() const { return get_max_speed_up_ms() * 100.0; }
225 :
226 : // Returns maximum climb rate in m/s used for shaping the vertical trajectory.
227 56683671 : float get_max_speed_up_ms() const { return _vel_max_up_ms; }
228 :
229 : /// Returns maximum descent rate in m/s (zero or positive).
230 55641286 : float get_max_speed_down_ms() const { return _vel_max_down_ms; }
231 :
232 : // Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent.
233 : // Used for takeoff or hold scenarios where downward motion is prohibited.
234 : void D_init_controller_no_descent();
235 :
236 : // Initializes U-axis controller to a stationary stopping point with zero velocity and acceleration.
237 : // Used when the trajectory starts at rest but the initial altitude is unspecified.
238 : // The resulting position target can be retrieved with get_pos_target_NED_m().
239 : void D_init_controller_stopping_point();
240 :
241 : // Smoothly decays U-axis acceleration to zero over time while maintaining current vertical velocity.
242 : // Reduces requested acceleration by ~95% every 0.5 seconds to avoid abrupt transitions.
243 : // `throttle_setting` is used to determine whether to preserve positive acceleration in low-thrust cases.
244 : void D_relax_controller(float throttle_setting);
245 :
246 : // Fully initializes the U-axis controller with current position, velocity, acceleration, and attitude.
247 : // Used during standard controller activation when full state is known.
248 : // Private function shared by other vertical initializers.
249 : void D_init_controller();
250 :
251 : // Sets the desired vertical acceleration in m/s² using jerk-limited shaping.
252 : // Smoothly transitions to the target acceleration from current kinematic state.
253 : // Constraints: max acceleration and jerk set via D_set_max_speed_accel_m().
254 : void input_accel_D_m(float accel_d_mss);
255 :
256 : // Sets desired vertical velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
257 : // Calculates required acceleration using current vertical kinematics.
258 : // If `limit_output` is true, limits apply to the combined (desired + correction) command.
259 : void input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output = true);
260 :
261 : // Generates a vertical trajectory using the given climb rate in cm/s and jerk-limited shaping.
262 : // Adjusts the internal target altitude based on integrated climb rate.
263 : // See D_set_pos_target_from_climb_rate_ms() for full details.
264 : void D_set_pos_target_from_climb_rate_cms(float climb_rate_cms);
265 :
266 : // Generates a vertical trajectory using the given climb rate in m/s and jerk-limited shaping.
267 : // Target altitude is updated over time by integrating the climb rate.
268 : void D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit = false);
269 :
270 : // Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping.
271 : // See input_pos_vel_accel_D_m() for full details.
272 : void input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output = true);
273 :
274 : // Sets vertical position, velocity, and acceleration in meters using jerk-limited shaping.
275 : // Calculates required acceleration using current state and constraints.
276 : // If `limit_output` is true, limits are applied to combined (desired + correction) command.
277 : void input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output = true);
278 :
279 : // Sets target altitude in cm using jerk-limited shaping to gradually move to the new position.
280 : // See D_set_alt_target_with_slew_m() for full details.
281 : void set_alt_target_with_slew_cm(float pos_u_cm);
282 :
283 : // Sets target altitude in meters using jerk-limited shaping.
284 : void D_set_alt_target_with_slew_m(float pos_u_m);
285 :
286 : // Returns true if the U-axis controller has run in the last 5 control loop cycles.
287 : bool D_is_active() const;
288 :
289 : // Runs the vertical (U-axis) position controller.
290 : // Computes output acceleration based on position and velocity errors using PID correction.
291 : // Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command.
292 : // Desired position, velocity, and acceleration must be set before calling.
293 : void D_update_controller();
294 :
295 :
296 :
297 : ///
298 : /// Accessors
299 : ///
300 :
301 : // Sets externally computed NED position, velocity, and acceleration in meters, m/s, and m/s².
302 : // Use when path planning or shaping is done outside this controller.
303 : void set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss);
304 :
305 : // Sets externally computed NE position, velocity, and acceleration in meters, m/s, and m/s².
306 : // Use when path planning or shaping is done outside this controller.
307 : void set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss);
308 :
309 :
310 : /// Position
311 :
312 : // Returns the estimated position in NED frame, in meters relative to EKF origin.
313 4931675 : const Vector3p& get_pos_estimate_NED_m() const { return _pos_estimate_ned_m; }
314 :
315 : // Returns estimated altitude above EKF origin in meters.
316 3824527 : float get_pos_estimate_U_m() const { return -_pos_estimate_ned_m.z; }
317 :
318 : // Returns the target position in NED frame, in meters relative to EKF origin.
319 40072 : const Vector3p& get_pos_target_NED_m() const { return _pos_target_ned_m; }
320 :
321 : // Sets the desired NE position in meters relative to EKF origin.
322 15 : void set_pos_desired_NE_m(const Vector2p& pos_desired_ne_m) { _pos_desired_ned_m.xy() = pos_desired_ne_m; }
323 :
324 : // Returns the desired position in NED frame, in meters relative to EKF origin.
325 4880061 : const Vector3p& get_pos_desired_NED_m() const { return _pos_desired_ned_m; }
326 :
327 : // Returns target altitude above EKF origin in centimeters.
328 : // See get_pos_target_U_m() for full details.
329 52342 : float get_pos_target_U_cm() const { return get_pos_target_U_m() * 100.0; }
330 :
331 : // Returns target altitude above EKF origin in meters.
332 615520 : float get_pos_target_U_m() const { return -_pos_target_ned_m.z; }
333 :
334 : // Sets desired altitude above EKF origin in centimeters.
335 : // See set_pos_desired_U_m() for full details.
336 2985 : void set_pos_desired_U_cm(float pos_desired_u_cm) { set_pos_desired_U_m(pos_desired_u_cm * 0.01); }
337 :
338 : // Sets desired altitude above EKF origin in meters.
339 3034 : void set_pos_desired_U_m(float pos_desired_u_m) { _pos_desired_ned_m.z = -pos_desired_u_m; }
340 :
341 : // Returns desired altitude above EKF origin in centimeters.
342 : // See get_pos_desired_U_m() for full details.
343 2870 : float get_pos_desired_U_cm() const { return get_pos_desired_U_m() * 100.0; }
344 :
345 : // Returns desired altitude above EKF origin in meters.
346 3129335 : float get_pos_desired_U_m() const { return -_pos_desired_ned_m.z; }
347 :
348 :
349 : /// Stopping Point
350 :
351 : // Computes NE stopping point in meters based on current position, velocity, and acceleration.
352 : void get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const;
353 :
354 : // Computes vertical stopping point relative to EKF origin in meters, Down-positive. based on current velocity and acceleration.
355 : void get_stopping_point_D_m(postype_t &stopping_point_d_m) const;
356 :
357 :
358 : /// Position Error
359 :
360 : // Returns NED position error vector in meters between current and target positions.
361 3294620 : const Vector3f get_pos_error_NED_m() const { return Vector3f{_p_pos_ne_m.get_error().x, _p_pos_ne_m.get_error().y, _p_pos_d_m.get_error()}; }
362 :
363 : // Returns total NE-plane position error magnitude in meters.
364 56637 : float get_pos_error_NE_m() const { return _p_pos_ne_m.get_error().length(); }
365 :
366 : // Returns vertical position error (altitude) in centimeters.
367 : // See get_pos_error_D_m() for full details.
368 52070 : float get_pos_error_U_cm() const { return -get_pos_error_D_m() * 100.0; }
369 :
370 : // Returns vertical position error (altitude) in meters.
371 465670 : float get_pos_error_D_m() const { return _p_pos_d_m.get_error(); }
372 :
373 :
374 : /// Velocity
375 :
376 : // Returns current velocity estimate in NED frame in m/s.
377 3723538 : const Vector3f& get_vel_estimate_NED_ms() const { return _vel_estimate_ned_ms; }
378 :
379 : // Returns current velocity estimate (Up) in m/s.
380 946987 : float get_vel_estimate_U_ms() const { return -_vel_estimate_ned_ms.z; }
381 :
382 : // Sets desired velocity in NED frame in cm/s.
383 : // See set_vel_desired_NED_ms() for full details.
384 0 : void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { set_vel_desired_NED_ms(Vector3f{vel_desired_neu_cms.x, vel_desired_neu_cms.y, -vel_desired_neu_cms.z} * 0.01); }
385 :
386 : // Sets desired velocity in NED frame in m/s.
387 0 : void set_vel_desired_NED_ms(const Vector3f &vel_desired_ned_ms) { _vel_desired_ned_ms = vel_desired_ned_ms; }
388 :
389 : // Sets desired horizontal (NE) velocity in m/s.
390 : void set_vel_desired_NE_ms(const Vector2f &vel_desired_ne_ms) { _vel_desired_ned_ms.xy() = vel_desired_ne_ms; }
391 :
392 : // Returns desired velocity in NEU frame in cm/s.
393 : // See get_vel_desired_NED_ms() for full details.
394 0 : const Vector3f get_vel_desired_NEU_cms() const { return Vector3f{_vel_desired_ned_ms.x, _vel_desired_ned_ms.y, -_vel_desired_ned_ms.z} * 100.0; }
395 :
396 : // Returns desired velocity in NED frame in m/s.
397 11533490 : const Vector3f& get_vel_desired_NED_ms() const { return _vel_desired_ned_ms; }
398 :
399 : // Returns desired velocity (Up) in m/s.
400 12848467 : float get_vel_desired_U_ms() const { return -_vel_desired_ned_ms.z; }
401 :
402 : // Returns velocity target in NEU frame in cm/s.
403 : // See get_vel_target_NED_ms() for full details.
404 398817 : const Vector3f get_vel_target_NEU_cms() const { return Vector3f{_vel_target_ned_ms.x, _vel_target_ned_ms.y, -_vel_target_ned_ms.z} * 100.0; }
405 :
406 : // Returns velocity target in NED frame in m/s.
407 1864882 : const Vector3f& get_vel_target_NED_ms() const { return _vel_target_ned_ms; }
408 :
409 : // Sets desired vertical velocity (Up) in m/s.
410 : void set_vel_desired_D_ms(float vel_desired_d_ms) { _vel_desired_ned_ms.z = vel_desired_d_ms; }
411 :
412 : // Returns vertical velocity target (Up) in cm/s.
413 : // See get_vel_target_U_ms() for full details.
414 52342 : float get_vel_target_U_cms() const { return get_vel_target_U_ms() * 100.0; }
415 :
416 : // Returns vertical velocity target (Up) in m/s.
417 820436 : float get_vel_target_U_ms() const { return -_vel_target_ned_ms.z; }
418 :
419 :
420 : /// Acceleration
421 :
422 : // Sets desired horizontal acceleration in m/s².
423 92 : void set_accel_desired_NE_mss(const Vector2f &accel_desired_ne_mss) { _accel_desired_ned_mss.xy() = accel_desired_ne_mss; }
424 :
425 : // Returns target NED acceleration in m/s².
426 21655020 : const Vector3f& get_accel_target_NED_mss() const { return _accel_target_ned_mss; }
427 :
428 :
429 : /// Terrain
430 :
431 : // Sets both the terrain altitude and terrain target to the same value
432 : // (altitude above EKF origin in centimeters, Up-positive).
433 : // See set_pos_terrain_target_D_m() for full description.
434 79725 : void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) { set_pos_terrain_target_D_m(-pos_terrain_target_u_cm * 0.01); }
435 :
436 : // Sets both the terrain altitude and terrain target to the same value
437 : // (relative to EKF origin in meters, Down-positive).
438 5170141 : void set_pos_terrain_target_D_m(float pos_terrain_target_d_m) { _pos_terrain_target_d_m = pos_terrain_target_d_m; }
439 :
440 : // Initializes both the terrain altitude and terrain target to the same value
441 : // (altitude above EKF origin in centimeters, Up-positive).
442 : // See init_pos_terrain_D_m() for full description.
443 : void init_pos_terrain_U_cm(float pos_terrain_u_cm);
444 :
445 : // Initializes both the terrain altitude and terrain target to the same value
446 : // (relative to EKF origin in meters, Down-positive).
447 : void init_pos_terrain_D_m(float pos_terrain_d_m);
448 :
449 : // Returns the current terrain altitude (Down-positive, relative to EKF origin, in meters).
450 20 : float get_pos_terrain_D_m() const { return _pos_terrain_d_m; }
451 :
452 : // Returns the current terrain correction velocity (Down-positive, m/s).
453 3518824 : float get_vel_terrain_D_ms() const { return _vel_terrain_d_ms; }
454 :
455 :
456 : /// Offset
457 :
458 : #if AP_SCRIPTING_ENABLED
459 : // Sets additional position, velocity, and acceleration offsets in meters (NED frame) for scripting.
460 : // Offsets are added to the controller’s internal target.
461 : // Used in LUA
462 : bool set_posvelaccel_offset(const Vector3f &pos_offset_NED_m, const Vector3f &vel_offset_NED_ms, const Vector3f &accel_offset_NED_mss);
463 :
464 : // Retrieves current scripted offsets in meters (NED frame).
465 : // Used in LUA
466 :
467 : bool get_posvelaccel_offset(Vector3f &pos_offset_NED_m, Vector3f &vel_offset_NED_ms, Vector3f &accel_offset_NED_mss);
468 :
469 : // Retrieves current target velocity (NED frame, m/s) including any scripted offset.
470 : // Used in LUA
471 : bool get_vel_target(Vector3f &vel_target_NED_ms);
472 :
473 : // Retrieves current target acceleration (NED frame, m/s²) including any scripted offset.
474 : // Used in LUA
475 : bool get_accel_target(Vector3f &accel_target_NED_mss);
476 : #endif
477 :
478 : // Sets NE offset targets in meters, m/s, and m/s².
479 : void set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss);
480 :
481 : // Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive.
482 : void set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, float accel_offset_target_d_mss);
483 :
484 : // Returns current NED position offset in meters.
485 4009316 : const Vector3p& get_pos_offset_NED_m() const { return _pos_offset_ned_m; }
486 :
487 : // Returns current NED velocity offset in m/s.
488 : const Vector3f& get_vel_offset_NED_ms() const { return _vel_offset_ned_ms; }
489 :
490 : // Returns current NED acceleration offset in m/s².
491 : const Vector3f& get_accel_offset_NED_mss() const { return _accel_offset_ned_mss; }
492 :
493 : // Sets vertical position offset in meters relative to EKF origin in meters, Down-positive.
494 : void set_pos_offset_D_m(float pos_offset_d_m) { _pos_offset_ned_m.z = pos_offset_d_m; }
495 :
496 : // Returns vertical position offset in meters relative to EKF origin in meters, Down-positive.
497 296 : float get_pos_offset_U_m() const { return -_pos_offset_ned_m.z; }
498 :
499 : // Returns vertical velocity offset in m/s.
500 4009054 : float get_vel_offset_D_ms() const { return _vel_offset_ned_ms.z; }
501 :
502 : // Returns vertical acceleration offset in m/s².
503 : float get_accel_offset_D_mss() const { return _accel_offset_ned_mss.z; }
504 :
505 : /// Outputs
506 :
507 : // Returns desired roll angle in radians for the attitude controller
508 2894276 : float get_roll_rad() const { return _roll_target_rad; }
509 :
510 : // Returns desired pitch angle in radians for the attitude controller.
511 2894276 : float get_pitch_rad() const { return _pitch_target_rad; }
512 :
513 : // Returns desired yaw angle in radians for the attitude controller.
514 10755743 : float get_yaw_rad() const { return _yaw_target_rad; }
515 :
516 : // Returns desired yaw rate in radians/second for the attitude controller.
517 1791569 : float get_yaw_rate_rads() const { return _yaw_rate_target_rads; }
518 :
519 : // Returns desired roll angle in centidegrees for the attitude controller.
520 : // See get_roll_rad() for full details.
521 3197476 : float get_roll_cd() const { return rad_to_cd(_roll_target_rad); }
522 :
523 : // Returns desired pitch angle in centidegrees for the attitude controller.
524 : // See get_pitch_rad() for full details.
525 3197476 : float get_pitch_cd() const { return rad_to_cd(_pitch_target_rad); }
526 :
527 : // Returns desired thrust direction as a unit vector in the body frame.
528 : Vector3f get_thrust_vector() const;
529 :
530 : // Returns bearing from current position to position target in radians.
531 : // 0 = North, positive = clockwise.
532 : float get_bearing_to_target_rad() const;
533 :
534 : // Returns the maximum allowed roll/pitch angle in radians.
535 : float get_lean_angle_max_rad() const;
536 :
537 : // Overrides the maximum allowed roll/pitch angle in radians.
538 : // A value of 0 reverts to using the ANGLE_MAX parameter.
539 1342917 : void set_lean_angle_max_rad(float angle_max_rad) { _angle_max_override_rad = angle_max_rad; }
540 :
541 : // Overrides the maximum allowed roll/pitch angle in degrees.
542 : // See set_lean_angle_max_rad() for full details.
543 0 : void set_lean_angle_max_deg(const float angle_max_deg) { set_lean_angle_max_rad(radians(angle_max_deg)); }
544 :
545 : // Overrides the maximum allowed roll/pitch angle in centidegrees.
546 : // See set_lean_angle_max_rad() for full details.
547 1342917 : void set_lean_angle_max_cd(const float angle_max_cd) { set_lean_angle_max_rad(cd_to_rad(angle_max_cd)); }
548 :
549 : /// Other
550 :
551 : // Returns reference to the NE position P controller.
552 5943901 : AC_P_2D& NE_get_pos_p() { return _p_pos_ne_m; }
553 :
554 : // Returns reference to the D (vertical) position P controller.
555 13474461 : AC_P_1D& D_get_pos_p() { return _p_pos_d_m; }
556 :
557 : // Returns reference to the NE velocity PID controller.
558 3695577 : AC_PID_2D& NE_get_vel_pid() { return _pid_vel_ne_m; }
559 :
560 : // Returns reference to the D (vertical) velocity PID controller.
561 99873 : AC_PID_Basic& D_get_vel_pid() { return _pid_vel_d_m; }
562 :
563 : // Returns reference to the D acceleration PID controller.
564 1381815 : AC_PID& D_get_accel_pid() { return _pid_accel_d_m; }
565 :
566 : // Marks that NE acceleration has been externally limited.
567 : // Prevents I-term windup by storing the current target direction.
568 1571714 : void NE_set_externally_limited() { _limit_vector_ned.x = _accel_target_ned_mss.x; _limit_vector_ned.y = _accel_target_ned_mss.y; }
569 :
570 : // Converts lean angles (rad) to NED acceleration in m/s².
571 : Vector3f lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const;
572 :
573 : // Writes position controller diagnostic logs (PSCN, PSCE, etc).
574 : void write_log();
575 :
576 : // Performs pre-arm checks for position control parameters and EKF readiness.
577 : // Returns false if failure_msg is populated.
578 : bool pre_arm_checks(const char *param_prefix,
579 : char *failure_msg,
580 : const uint8_t failure_msg_len);
581 :
582 : // Enables or disables vibration compensation mode.
583 : // When enabled, disables use of horizontal velocity estimates.
584 1 : void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
585 :
586 : // Reset handling method
587 : enum class EKFResetMethod : uint8_t {
588 : MoveTarget = 0, // the target position is reset so the vehicle does not physically move
589 : MoveVehicle = 1 // the target position is smoothly transitioned so the vehicle moves to its previous position coordinates
590 : };
591 30412448 : void set_reset_handling_method(EKFResetMethod reset_method) { _ekf_reset_method = reset_method; }
592 :
593 : // Returns confidence (0–1) in vertical control authority based on output usage.
594 : // Used to assess throttle margin and PID effectiveness.
595 1303597 : float get_vel_D_control_ratio() const { return constrain_float(_vel_d_control_ratio, 0.0f, 1.0f); }
596 :
597 : // Returns lateral distance to closest point on active trajectory in meters.
598 : // Used to assess horizontal deviation from path.
599 : float crosstrack_error_m() const;
600 :
601 : // Resets NED position controller state to prevent transients when exiting standby.
602 : // Zeros I-terms and aligns targets to current position.
603 : void NED_standby_reset();
604 :
605 : // Returns measured vertical (Down) acceleration in m/s² (Earth frame, gravity-compensated).
606 : // Positive = downward acceleration.
607 38835215 : float get_estimated_accel_D_mss() const { return _ahrs.get_accel_ef().z + GRAVITY_MSS; }
608 :
609 : // Returns measured vertical (Up) acceleration in m/s² (Earth frame, gravity-compensated).
610 : // Positive = upward acceleration.
611 136507 : float get_estimated_accel_U_mss() const { return -get_estimated_accel_D_mss(); }
612 :
613 : // Returns true if the requested forward pitch is limited by the configured tilt constraint.
614 : bool get_fwd_pitch_is_limited() const;
615 :
616 : // Sets artificial NE position disturbance in meters.
617 0 : void set_disturb_pos_NE_m(const Vector2f& disturb_pos_m) { _disturb_pos_ne_m = disturb_pos_m; }
618 :
619 : // Sets artificial NE velocity disturbance in m/s.
620 0 : void set_disturb_vel_NE_ms(const Vector2f& disturb_vel_ms) { _disturb_vel_ne_ms = disturb_vel_ms; }
621 :
622 : // Logs position controller state along the North axis to PSCN..
623 : // Logs desired, target, and actual position [m], velocity [m/s], and acceleration [m/s²].
624 : static void Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
625 :
626 : // Logs position controller state along the East axis to PSCE.
627 : // Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
628 : static void Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
629 :
630 : // Logs position controller state along the Down (vertical) axis to PSCD.
631 : // Logs desired, target, and actual values for position [m], velocity [m/s], and acceleration [m/s²].
632 : static void Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss);
633 :
634 : // Logs offset tracking along the North axis to PSON.
635 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
636 : static void Write_PSON(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
637 :
638 : // Logs offset tracking along the East axis to PSOE.
639 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
640 : static void Write_PSOE(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
641 :
642 : // Logs offset tracking along the Down axis to PSOD.
643 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
644 : static void Write_PSOD(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
645 :
646 : // Logs terrain-following offset tracking along the Down axis to PSOT.
647 : // Logs target and actual offset for position [m], velocity [m/s], and acceleration [m/s²].
648 : static void Write_PSOT(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss);
649 :
650 : // perform any required parameter conversions
651 : void convert_parameters();
652 :
653 : // Returns pointer to the global AC_PosControl singleton.
654 1270 : static AC_PosControl *get_singleton(void) { return _singleton; }
655 :
656 : static const struct AP_Param::GroupInfo var_info[];
657 :
658 : protected:
659 :
660 : // Calculates vertical throttle using vibration-resistant feedforward estimation.
661 : // Returns throttle output using manual feedforward gain for vibration compensation mode.
662 : // Integrator is adjusted using velocity error when PID is being overridden.
663 : float get_throttle_with_vibration_override();
664 :
665 : // Converts horizontal acceleration (m/s²) to roll/pitch lean angles in radians.
666 : void accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const;
667 :
668 : // Converts current target lean angles to NE acceleration in m/s².
669 : void lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const;
670 :
671 : // Computes desired yaw and yaw rate based on the NE acceleration and velocity vectors.
672 : // Aligns yaw with the direction of travel if speed exceeds 5% of maximum.
673 : void calculate_yaw_and_rate_yaw();
674 :
675 : // Computes scaling factor to increase max vertical accel/jerk if vertical speed exceeds configured limits.
676 : float calculate_overspeed_gain();
677 :
678 :
679 : /// Terrain Following
680 :
681 : // Initializes terrain position, velocity, and acceleration to match the terrain target.
682 : void init_terrain();
683 :
684 : // Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants.
685 : void update_terrain();
686 :
687 :
688 : /// Offsets
689 :
690 : // Initializes NE position/velocity/acceleration offsets to match their respective targets.
691 : void NE_init_offsets();
692 :
693 : // Initializes vertical (D) offsets to match their respective targets.
694 : void D_init_offsets();
695 :
696 : // Updates NE offsets by gradually moving them toward their targets.
697 : void NE_update_offsets();
698 :
699 : // Updates vertical (D) offsets by gradually moving them toward their targets.
700 : void D_update_offsets();
701 :
702 : // Initializes tracking of NE EKF position resets.
703 : void NE_init_ekf_reset();
704 :
705 : // Handles NE position reset detection and response (e.g., clearing accumulated errors).
706 : void NE_handle_ekf_reset();
707 :
708 : // Initializes tracking of vertical (D) EKF resets.
709 : void D_init_ekf_reset();
710 :
711 : // Handles the vertical (D) EKF reset detection and response.
712 : void D_handle_ekf_reset();
713 :
714 : // references to inertial nav and ahrs libraries
715 : AP_AHRS_View& _ahrs;
716 : const class AP_Motors& _motors;
717 : AC_AttitudeControl& _attitude_control;
718 :
719 : // parameters
720 : AP_Float _lean_angle_max_deg; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
721 : AP_Float _shaping_jerk_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
722 : AP_Float _shaping_jerk_d_msss; // Jerk limit of the u kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
723 : AC_P_2D _p_pos_ne_m; // XY axis position controller to convert target distance (m) to target velocity (m/s)
724 : AC_P_1D _p_pos_d_m; // Z axis position controller to convert target altitude (m) to target climb rate (m/s)
725 : AC_PID_2D _pid_vel_ne_m; // XY axis velocity controller to convert target velocity (m/s) to target acceleration (m/s²)
726 : AC_PID_Basic _pid_vel_d_m; // Z axis velocity controller to convert target climb rate (m/s) to target acceleration (m/s²)
727 : AC_PID _pid_accel_d_m; // Z axis acceleration controller to convert target acceleration (in units of gravity) to normalised throttle output
728 :
729 : // internal variables
730 : float _dt_s; // time difference (in seconds) since the last loop time
731 : uint32_t _last_update_ne_ticks; // ticks of last NE_update_controller call
732 : uint32_t _last_update_d_ticks; // ticks of last update_z_controller call
733 : float _vel_max_ne_ms; // max horizontal speed in m/s used for kinematic shaping
734 : float _vel_max_up_ms; // max climb rate in m/s used for kinematic shaping
735 : float _vel_max_down_ms; // max descent rate in m/s used for kinematic shaping
736 : float _accel_max_ne_mss; // max horizontal acceleration in m/s² used for kinematic shaping
737 : float _accel_max_d_mss; // max vertical acceleration in m/s² used for kinematic shaping
738 : float _jerk_max_ne_msss; // Jerk limit of the ne kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
739 : float _jerk_max_d_msss; // Jerk limit of the z kinematic path generation in m/s³ used to determine how quickly the aircraft varies the acceleration target
740 : float _vel_d_control_ratio = 2.0f;// confidence that we have control in the vertical axis
741 : Vector2f _disturb_pos_ne_m; // position disturbance generated by system ID mode
742 : Vector2f _disturb_vel_ne_ms; // velocity disturbance generated by system ID mode
743 : float _ne_control_scale_factor = 1.0; // single loop scale factor for XY control
744 :
745 : // output from controller
746 : float _roll_target_rad; // desired roll angle in radians calculated by position controller
747 : float _pitch_target_rad; // desired roll pitch in radians calculated by position controller
748 : float _yaw_target_rad; // desired yaw in radians calculated by position controller
749 : float _yaw_rate_target_rads; // desired yaw rate in radians per second calculated by position controller
750 :
751 : // position controller internal variables
752 : Vector3p _pos_estimate_ned_m; // estimated location, frame NED in m relative to the EKF origin.
753 : Vector3p _pos_desired_ned_m; // desired location, frame NED in m relative to the EKF origin. This is equal to the _pos_target_ned_m minus offsets
754 : Vector3p _pos_target_ned_m; // target location, frame NED in m relative to the EKF origin. This is equal to the _pos_desired_ned_m plus offsets
755 : Vector3f _vel_estimate_ned_ms; // estimated velocity in NED m/s
756 : Vector3f _vel_desired_ned_ms; // desired velocity in NED m/s
757 : Vector3f _vel_target_ned_ms; // velocity target in NED m/s calculated by pos_to_rate step
758 : Vector3f _accel_desired_ned_mss; // desired acceleration in NED m/s² (feed forward)
759 : Vector3f _accel_target_ned_mss; // acceleration target in NED m/s²
760 : // todo: seperate the limit vector into ne and u. ne is based on acceleration while u is set +-1 based on throttle saturation. Together they don't form a direction vector because the units are different.
761 : Vector3f _limit_vector_ned; // the direction that the position controller is limited, zero when not limited
762 :
763 : // terrain handling variables
764 : float _pos_terrain_target_d_m; // position terrain target in m relative to the EKF origin in NED frame
765 : float _pos_terrain_d_m; // position terrain in m from the EKF origin in NED frame. This terrain moves towards _pos_terrain_target_d_m
766 : float _vel_terrain_d_ms; // velocity terrain in NED m/s calculated by pos_to_rate step. This terrain moves towards _vel_terrain_target
767 : float _accel_terrain_d_mss; // acceleration terrain in NED m/s²
768 :
769 : // offset handling variables
770 : Vector3p _pos_offset_target_ned_m; // position offset target in m relative to the EKF origin in NED frame
771 : Vector3p _pos_offset_ned_m; // position offset in m from the EKF origin in NED frame. This offset moves towards _pos_offset_target_ned_m
772 : Vector3f _vel_offset_target_ned_ms; // velocity offset target in m/s in NED frame
773 : Vector3f _vel_offset_ned_ms; // velocity offset in NED m/s calculated by pos_to_rate step. This offset moves towards _vel_offset_target_ned_ms
774 : Vector3f _accel_offset_target_ned_mss; // acceleration offset target in m/s² in NED frame
775 : Vector3f _accel_offset_ned_mss; // acceleration offset in NED m/s²
776 : uint32_t _posvelaccel_offset_target_ne_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
777 : uint32_t _posvelaccel_offset_target_d_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
778 :
779 : // ekf reset handling
780 : uint16_t _ahrs_position_NE_reset_count; // count of ekf ne position resets
781 : uint16_t _ahrs_position_D_reset_count; // count of ekf altitude resets
782 : EKFResetMethod _ekf_reset_method = EKFResetMethod::MoveTarget; // EKF reset handling method. Loiter should use MoveTarget, Auto should use MoveVehicle
783 :
784 : // high vibration handling
785 : bool _vibe_comp_enabled; // true when high vibration compensation is on
786 :
787 : // angle max override, if zero then use ANGLE_MAX parameter
788 : float _angle_max_override_rad;
789 :
790 : // return true if on a real vehicle or SITL with lock-step scheduling
791 : bool has_good_timing(void) const;
792 :
793 : private:
794 : // Internal log writer for PSCx (North, East, Down tracking).
795 : // Reduces duplication between Write_PSCN, PSCE, and PSCD.
796 : // Used for logging desired/target/actual position, velocity, and acceleration per axis.
797 : static void Write_PSCx(LogMessages ID, float pos_desired_m, float pos_target_m, float pos_m,
798 : float vel_desired_ms, float vel_target_ms, float vel_ms,
799 : float accel_desired_mss, float accel_target_mss, float accel_mss);
800 :
801 : // Internal log writer for PSOx (North, East, Down tracking).
802 : // Reduces duplication between Write_PSON, PSOE, and PSOD.
803 : // Used for logging the target/actual position, velocity, and acceleration offset per axis.
804 : static void Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m,
805 : float vel_target_offset_ms, float vel_offset_ms,
806 : float accel_target_offset_mss, float accel_offset_mss);
807 :
808 : // singleton
809 : static AC_PosControl *_singleton;
810 : };
|
