Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include "AC_PosControl.h"
3 : #include <AP_Math/AP_Math.h>
4 : #include <AP_Logger/AP_Logger.h>
5 : #include <AP_Motors/AP_Motors.h> // motors library
6 : #include <AP_Vehicle/AP_Vehicle_Type.h>
7 : #include <AP_Scheduler/AP_Scheduler.h>
8 :
9 : extern const AP_HAL::HAL& hal;
10 :
11 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
12 : // default gains for Plane
13 : # define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default
14 : # define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default
15 : # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
16 : # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
17 : # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
18 : # define POSCONTROL_D_ACC_P 0.03f // vertical acceleration controller P gain default
19 : # define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default
20 : # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
21 : # define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default
22 : # define POSCONTROL_D_ACC_FILT_HZ 10.0f // vertical acceleration controller input filter default
23 : # define POSCONTROL_D_ACC_DT 0.02f // vertical acceleration controller dt default
24 : # define POSCONTROL_NE_POS_P 0.5f // horizontal position controller P gain default
25 : # define POSCONTROL_NE_VEL_P 0.7f // horizontal velocity controller P gain default
26 : # define POSCONTROL_NE_VEL_I 0.35f // horizontal velocity controller I gain default
27 : # define POSCONTROL_NE_VEL_D 0.17f // horizontal velocity controller D gain default
28 : # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
29 : # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
30 : # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
31 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
32 : // default gains for Sub
33 : # define POSCONTROL_D_POS_P 3.0f // vertical position controller P gain default
34 : # define POSCONTROL_D_VEL_P 8.0f // vertical velocity controller P gain default
35 : # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
36 : # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
37 : # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
38 : # define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default
39 : # define POSCONTROL_D_ACC_I 0.01f // vertical acceleration controller I gain default
40 : # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
41 : # define POSCONTROL_D_ACC_IMAX 0.1f // vertical acceleration controller IMAX gain default
42 : # define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default
43 : # define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default
44 : # define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default
45 : # define POSCONTROL_NE_VEL_P 1.0f // horizontal velocity controller P gain default
46 : # define POSCONTROL_NE_VEL_I 0.5f // horizontal velocity controller I gain default
47 : # define POSCONTROL_NE_VEL_D 0.0f // horizontal velocity controller D gain default
48 : # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
49 : # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
50 : # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
51 : #else
52 : // default gains common between TradHeli and Copter
53 : # define POSCONTROL_D_POS_P 1.0f // vertical position controller P gain default
54 : # define POSCONTROL_D_VEL_P 5.0f // vertical velocity controller P gain default
55 : # define POSCONTROL_D_VEL_IMAX 10.0f // vertical velocity controller IMAX gain default
56 : # define POSCONTROL_D_VEL_FILT_HZ 5.0f // vertical velocity controller input filter
57 : # define POSCONTROL_D_VEL_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
58 : # define POSCONTROL_D_ACC_I 0.1f // vertical acceleration controller I gain default
59 : # define POSCONTROL_D_ACC_D 0.0f // vertical acceleration controller D gain default
60 : # define POSCONTROL_D_ACC_IMAX 0.8f // vertical acceleration controller IMAX gain default
61 : # define POSCONTROL_D_ACC_FILT_HZ 20.0f // vertical acceleration controller input filter default
62 : # define POSCONTROL_D_ACC_DT 0.0025f // vertical acceleration controller dt default
63 : # define POSCONTROL_NE_POS_P 1.0f // horizontal position controller P gain default
64 : # define POSCONTROL_NE_VEL_IMAX 10.0f // horizontal velocity controller IMAX gain default
65 : # define POSCONTROL_NE_VEL_FILT_HZ 5.0f // horizontal velocity controller input filter
66 : # define POSCONTROL_NE_VEL_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
67 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
68 : // default gains specific to TradHeli
69 : # define POSCONTROL_D_ACC_P 0.028f // vertical acceleration controller P gain default
70 : # define POSCONTROL_NE_VEL_P 1.0f // horizontal velocity controller P gain default
71 : # define POSCONTROL_NE_VEL_I 0.5f // horizontal velocity controller I gain default
72 : # define POSCONTROL_NE_VEL_D 0.0f // horizontal velocity controller D gain default
73 : #else
74 : // default gains specific to Copter
75 : # define POSCONTROL_D_ACC_P 0.05f // vertical acceleration controller P gain default
76 : # define POSCONTROL_NE_VEL_P 2.0f // horizontal velocity controller P gain default
77 : # define POSCONTROL_NE_VEL_I 1.0f // horizontal velocity controller I gain default
78 : # define POSCONTROL_NE_VEL_D 0.25f // horizontal velocity controller D gain default
79 : #endif
80 : #endif
81 :
82 : // vibration compensation gains
83 : #define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
84 : #define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
85 :
86 : // velocity offset targets timeout if not updated within 3 seconds
87 : #define POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS 3000
88 :
89 : AC_PosControl *AC_PosControl::_singleton;
90 :
91 : const AP_Param::GroupInfo AC_PosControl::var_info[] = {
92 : // 0 was used for HOVER
93 :
94 : // POS_ACC_XY_FILT was here.
95 :
96 : // @Param: _D_POS_P
97 : // @DisplayName: Position (vertical) controller P gain
98 : // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller. Previously _POSZ_P.
99 : // @Range: 0.50 4.00
100 : // @Increment: 0.01
101 : // @User: Standard
102 : AP_SUBGROUPINFO(_p_pos_d_m, "_D_POS_", 2, AC_PosControl, AC_P_1D),
103 :
104 : // 3 was _VELZ_ which has become _D_VEL_
105 :
106 : // 4 was _ACCZ_ which has become _D_ACC_
107 :
108 : // @Param: _NE_POS_P
109 : // @DisplayName: Position (horizontal) controller P gain
110 : // @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller. Previously _POSXY_P.
111 : // @Range: 0.50 4.00
112 : // @Increment: 0.01
113 : // @User: Standard
114 : AP_SUBGROUPINFO(_p_pos_ne_m, "_NE_POS_", 5, AC_PosControl, AC_P_2D),
115 :
116 : // 6 was _VELXY_ which has become _NE_VEL_
117 :
118 : // @Param: _ANGLE_MAX
119 : // @DisplayName: Position Control Angle Max
120 : // @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value
121 : // @Units: deg
122 : // @Range: 0 45
123 : // @Increment: 1
124 : // @User: Advanced
125 : AP_GROUPINFO("_ANGLE_MAX", 7, AC_PosControl, _lean_angle_max_deg, 0.0f),
126 :
127 : // IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate
128 :
129 : // @Param: _NE_JERK
130 : // @DisplayName: Jerk limit for the horizontal kinematic input shaping
131 : // @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target
132 : // @Units: m/s/s/s
133 : // @Range: 1 50
134 : // @Increment: 1
135 : // @User: Advanced
136 : AP_GROUPINFO("_NE_JERK", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE_MSSS),
137 :
138 : // @Param: _D_JERK
139 : // @DisplayName: Jerk limit for the vertical kinematic input shaping
140 : // @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target
141 : // @Units: m/s/s/s
142 : // @Range: 1 50
143 : // @Increment: 1
144 : // @User: Advanced
145 : AP_GROUPINFO("_D_JERK", 11, AC_PosControl, _shaping_jerk_d_msss, POSCONTROL_JERK_D_MSSS),
146 :
147 : // @Param: _D_VEL_P
148 : // @DisplayName: Velocity (vertical) controller P gain
149 : // @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller. Previously _VELZ_P.
150 : // @Range: 1.0 10.0
151 : // @Increment: 0.1
152 : // @User: Standard
153 :
154 : // @Param: _D_VEL_I
155 : // @DisplayName: Velocity (vertical) controller I gain
156 : // @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration. Previously _VELZ_I.
157 : // @Range: 0.00 10.0
158 : // @Increment: 0.1
159 : // @User: Advanced
160 :
161 : // @Param: _D_VEL_IMAX
162 : // @DisplayName: Velocity (vertical) controller I gain maximum
163 : // @Description: Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELZ_IMAX * 0.01.
164 : // @Range: 1.000 10.000
165 : // @Increment: 0.1
166 : // @User: Standard
167 :
168 : // @Param: _D_VEL_D
169 : // @DisplayName: Velocity (vertical) controller D gain
170 : // @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity. Previously _VELZ_D.
171 : // @Range: 0.00 2.00
172 : // @Increment: 0.01
173 : // @User: Advanced
174 :
175 : // @Param: _D_VEL_FF
176 : // @DisplayName: Velocity (vertical) controller Feed Forward gain
177 : // @Description: Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target. Previously _VELZ_FF.
178 : // @Range: 0.00 2.00
179 : // @Increment: 0.01
180 : // @User: Advanced
181 :
182 : // @Param: _D_VEL_FLTE
183 : // @DisplayName: Velocity (vertical) error filter
184 : // @Description: Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELZ_FLTE.
185 : // @Range: 0 100
186 : // @Increment: 1.0
187 : // @Units: Hz
188 : // @User: Advanced
189 :
190 : // @Param: _D_VEL_FLTD
191 : // @DisplayName: Velocity (vertical) input filter for D term
192 : // @Description: Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms. Previously _VELZ_FLTD.
193 : // @Range: 0 100
194 : // @Increment: 1.0
195 : // @Units: Hz
196 : // @User: Advanced
197 : AP_SUBGROUPINFO(_pid_vel_d_m, "_D_VEL_", 12, AC_PosControl, AC_PID_Basic),
198 :
199 : // @Param: _D_ACC_P
200 : // @DisplayName: Acceleration (vertical) controller P gain
201 : // @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output. If upgrading from 4.6 this is _ACCZ_P * 0.1.
202 : // @Range: 0.010 0.250
203 : // @Increment: 0.001
204 : // @User: Standard
205 :
206 : // @Param: _D_ACC_I
207 : // @DisplayName: Acceleration (vertical) controller I gain
208 : // @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration. If upgrading from 4.6 this is _ACCZ_I * 0.1.
209 : // @Range: 0.000 0.500
210 : // @Increment: 0.001
211 : // @User: Standard
212 :
213 : // @Param: _D_ACC_IMAX
214 : // @DisplayName: Acceleration (vertical) controller I gain maximum
215 : // @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate. If upgrading from 4.6 this is _ACCZ_IMAX * 0.001.
216 : // @Range: 0.0 1.0
217 : // @Increment: 0.01
218 : // @Units: d%
219 : // @User: Standard
220 :
221 : // @Param: _D_ACC_D
222 : // @DisplayName: Acceleration (vertical) controller D gain
223 : // @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration. If upgrading from 4.6 this is _ACCZ_D * 0.1.
224 : // @Range: 0.000 0.100
225 : // @Increment: 0.001
226 : // @User: Standard
227 :
228 : // @Param: _D_ACC_FF
229 : // @DisplayName: Acceleration (vertical) controller feed forward
230 : // @Description: Acceleration (vertical) controller feed forward. If upgrading from 4.6 this is _ACCZ_FF * 0.1.
231 : // @Range: 0.000 0.100
232 : // @Increment: 0.001
233 : // @User: Standard
234 :
235 : // @Param: _D_ACC_FLTT
236 : // @DisplayName: Acceleration (vertical) controller target frequency in Hz
237 : // @Description: Acceleration (vertical) controller target frequency in Hz. Previously _ACCZ_FLTT.
238 : // @Range: 1 50
239 : // @Increment: 1
240 : // @Units: Hz
241 : // @User: Standard
242 :
243 : // @Param: _D_ACC_FLTE
244 : // @DisplayName: Acceleration (vertical) controller error frequency in Hz
245 : // @Description: Acceleration (vertical) controller error frequency in Hz. Previously _ACCZ_FLTE.
246 : // @Range: 1 100
247 : // @Increment: 1
248 : // @Units: Hz
249 : // @User: Standard
250 :
251 : // @Param: _D_ACC_FLTD
252 : // @DisplayName: Acceleration (vertical) controller derivative frequency in Hz
253 : // @Description: Acceleration (vertical) controller derivative frequency in Hz. Previously _ACCZ_FLTD.
254 : // @Range: 1 100
255 : // @Increment: 1
256 : // @Units: Hz
257 : // @User: Standard
258 :
259 : // @Param: _D_ACC_SMAX
260 : // @DisplayName: Accel (vertical) slew rate limit
261 : // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
262 : // @Range: 0 100
263 : // @Increment: 0.1
264 : // @User: Advanced
265 :
266 : // @Param: _D_ACC_PDMX
267 : // @DisplayName: Acceleration (vertical) controller PD sum maximum
268 : // @Description: Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output. If upgrading from 4.6 this is _ACCZ_P * 0.1.
269 : // @Range: 0.00 1.00
270 : // @Increment: 0.01
271 : // @Units: d%
272 :
273 : // @Param: _D_ACC_D_FF
274 : // @DisplayName: Accel (vertical) Derivative FeedForward Gain
275 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target. If upgrading from 4.6 this is _ACCZ_P * 0.1.
276 : // @Range: 0.000 0.050
277 : // @Increment: 0.001
278 : // @User: Advanced
279 :
280 : // @Param: _D_ACC_NTF
281 : // @DisplayName: Accel (vertical) Target notch filter index
282 : // @Description: Accel (vertical) Target notch filter index, zero disables. If upgrading from 4.6 this is Previously _ACCZ_NTF.
283 : // @Range: 0 8
284 : // @User: Advanced
285 :
286 : // @Param: _D_ACC_NEF
287 : // @DisplayName: Accel (vertical) Error notch filter index
288 : // @Description: Accel (vertical) Error notch filter index, zero disables. If upgrading from 4.6 this is Previously _ACCZ_NEF.
289 : // @Range: 0 8
290 : // @User: Advanced
291 : AP_SUBGROUPINFO(_pid_accel_d_m, "_D_ACC_", 13, AC_PosControl, AC_PID),
292 :
293 : // @Param: _NE_VEL_P
294 : // @DisplayName: Velocity (horizontal) P gain
295 : // @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration. Previously _VELXY_P.
296 : // @Range: 0.10 10.00
297 : // @Increment: 0.01
298 : // @User: Advanced
299 :
300 : // @Param: _NE_VEL_I
301 : // @DisplayName: Velocity (horizontal) I gain
302 : // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration. Previously _VELXY_I.
303 : // @Range: 0.10 10.00
304 : // @Increment: 0.01
305 : // @User: Advanced
306 :
307 : // @Param: _NE_VEL_D
308 : // @DisplayName: Velocity (horizontal) D gain
309 : // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity. Previously _VELXY_D.
310 : // @Range: 0.00 1.00
311 : // @Increment: 0.01
312 : // @User: Advanced
313 :
314 : // @Param: _NE_VEL_IMAX
315 : // @DisplayName: Velocity (horizontal) integrator maximum
316 : // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output. If upgrading from 4.6 this is _VELXY_IMAX * 0.01.
317 : // @Range: 0 10
318 : // @Increment: 1
319 : // @Units: m/s/s
320 : // @User: Advanced
321 :
322 : // @Param: _NE_VEL_FLTE
323 : // @DisplayName: Velocity (horizontal) input filter
324 : // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms. Previously _VELXY_FLTE.
325 : // @Range: 0 100
326 : // @Increment: 1
327 : // @Units: Hz
328 : // @User: Advanced
329 :
330 : // @Param: _NE_VEL_FLTD
331 : // @DisplayName: Velocity (horizontal) input filter
332 : // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term. Previously _VELXY_FLTD.
333 : // @Range: 0 100
334 : // @Increment: 1
335 : // @Units: Hz
336 : // @User: Advanced
337 :
338 : // @Param: _NE_VEL_FF
339 : // @DisplayName: Velocity (horizontal) feed forward gain
340 : // @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration. Previously _VELXY_FF.
341 : // @Range: 0.10 10.00
342 : // @Increment: 0.01
343 : // @User: Advanced
344 : AP_SUBGROUPINFO(_pid_vel_ne_m, "_NE_VEL_", 14, AC_PosControl, AC_PID_2D),
345 :
346 : AP_GROUPEND
347 : };
348 :
349 : // Default constructor.
350 : // Note that the Vector/Matrix constructors already implicitly zero
351 : // their values.
352 : //
353 1285 : AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
354 1285 : _ahrs(ahrs),
355 1285 : _motors(motors),
356 1285 : _attitude_control(attitude_control),
357 1285 : _p_pos_ne_m(POSCONTROL_NE_POS_P),
358 1285 : _p_pos_d_m(POSCONTROL_D_POS_P),
359 1285 : _pid_vel_ne_m(POSCONTROL_NE_VEL_P, POSCONTROL_NE_VEL_I, POSCONTROL_NE_VEL_D, 0.0f, POSCONTROL_NE_VEL_IMAX, POSCONTROL_NE_VEL_FILT_HZ, POSCONTROL_NE_VEL_FILT_D_HZ),
360 1285 : _pid_vel_d_m(POSCONTROL_D_VEL_P, 0.0f, 0.0f, 0.0f, POSCONTROL_D_VEL_IMAX, POSCONTROL_D_VEL_FILT_HZ, POSCONTROL_D_VEL_FILT_D_HZ),
361 1285 : _pid_accel_d_m(POSCONTROL_D_ACC_P, POSCONTROL_D_ACC_I, POSCONTROL_D_ACC_D, 0.0f, POSCONTROL_D_ACC_IMAX, 0.0f, POSCONTROL_D_ACC_FILT_HZ, 0.0f),
362 1285 : _vel_max_ne_ms(POSCONTROL_SPEED_MS),
363 1285 : _vel_max_up_ms(POSCONTROL_SPEED_UP_MS),
364 1285 : _vel_max_down_ms(POSCONTROL_SPEED_DOWN_MS),
365 1285 : _accel_max_ne_mss(POSCONTROL_ACCEL_NE_MSS),
366 1285 : _accel_max_d_mss(POSCONTROL_ACCEL_D_MSS),
367 1285 : _jerk_max_ne_msss(POSCONTROL_JERK_NE_MSSS),
368 2570 : _jerk_max_d_msss(POSCONTROL_JERK_D_MSSS)
369 : {
370 1285 : AP_Param::setup_object_defaults(this, var_info);
371 :
372 1285 : _singleton = this;
373 1285 : }
374 :
375 :
376 : ///
377 : /// 3D position shaper
378 : ///
379 :
380 : // Sets a new NED position target in meters and computes a jerk-limited trajectory.
381 : // Updates internal acceleration commands using a smooth kinematic path constrained
382 : // by configured acceleration and jerk limits.
383 : // The path can be offset vertically to follow the terrain by providing the current
384 : // terrain level in the NED frame and the terrain margin. Terrain margin is used to
385 : // constrain horizontal velocity to avoid vertical buffer violation.
386 739565 : void AC_PosControl::input_pos_NED_m(const Vector3p& pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m)
387 : {
388 : // Terrain following velocity scalar must be calculated before we remove the position offset
389 739565 : const float offset_d_scalar = terrain_scaler_D_m(pos_terrain_target_d_m, terrain_margin_m);
390 739565 : set_pos_terrain_target_D_m(pos_terrain_target_d_m);
391 :
392 : // calculated increased maximum acceleration and jerk if over speed
393 739565 : const float overspeed_gain = calculate_overspeed_gain();
394 739565 : const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
395 739565 : const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
396 :
397 739565 : update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
398 :
399 : // adjust desired altitude if motors have not hit their limits
400 739565 : update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
401 :
402 : // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos_ne_m
403 739565 : float vel_max_ne_ms = 0.0f;
404 739565 : float vel_max_d_ms = 0.0f;
405 739565 : Vector3f travel_dir_unit = (pos_ned_m - _pos_desired_ned_m).tofloat();
406 739565 : if (is_positive(travel_dir_unit.length_squared()) ) {
407 620123 : travel_dir_unit.normalize();
408 620123 : float travel_dir_unit_ne_length = travel_dir_unit.xy().length();
409 :
410 620123 : float vel_max_ms = kinematic_limit(travel_dir_unit, _vel_max_ne_ms, _vel_max_up_ms, _vel_max_down_ms);
411 620123 : vel_max_ne_ms = vel_max_ms * travel_dir_unit_ne_length;
412 620123 : vel_max_d_ms = fabsf(vel_max_ms * travel_dir_unit.z);
413 : }
414 :
415 : // reduce speed if we are reaching the edge of our vertical buffer
416 739565 : vel_max_ne_ms *= offset_d_scalar;
417 :
418 739565 : Vector2f vel_ne_ms;
419 739565 : Vector2f accel_ne_mss;
420 739565 : shape_pos_vel_accel_xy(pos_ned_m.xy(), vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
421 : vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false);
422 :
423 739565 : float pos_d_m = pos_ned_m.z;
424 739565 : shape_pos_vel_accel(pos_d_m, 0, 0,
425 739565 : _pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
426 : -vel_max_d_ms, vel_max_d_ms,
427 : -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
428 : jerk_max_d_msss, _dt_s, false);
429 739565 : }
430 :
431 : // Returns a scaling factor for horizontal velocity in m/s to ensure
432 : // the vertical controller maintains a safe distance above terrain.
433 4748614 : float AC_PosControl::terrain_scaler_D_m(float pos_terrain_d_m, float terrain_margin_m) const
434 : {
435 4748614 : if (is_zero(terrain_margin_m)) {
436 682850 : return 1.0;
437 : }
438 4065764 : float pos_offset_error_d_m = _pos_estimate_ned_m.z - (_pos_target_ned_m.z + (pos_terrain_d_m - _pos_terrain_d_m));
439 4065764 : return constrain_float((1.0 - (fabsf(pos_offset_error_d_m) - 0.5 * terrain_margin_m) / (0.5 * terrain_margin_m)), 0.01, 1.0);
440 : }
441 :
442 : ///
443 : /// Lateral position controller
444 : ///
445 :
446 : // Sets maximum horizontal speed (cm/s) and acceleration (cm/s²) for NE-axis shaping.
447 : // Can be called anytime; transitions are handled smoothly.
448 : // All arguments should be positive.
449 : // See NE_set_max_speed_accel_m() for full details.
450 2936 : void AC_PosControl::NE_set_max_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss)
451 : {
452 2936 : NE_set_max_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01);
453 2936 : }
454 :
455 : // Sets maximum horizontal speed (m/s) and acceleration (m/s²) for NE-axis shaping.
456 : // These values constrain the kinematic trajectory used by the lateral controller.
457 : // All arguments should be positive.
458 1477059 : void AC_PosControl::NE_set_max_speed_accel_m(float speed_ne_ms, float accel_ne_mss)
459 : {
460 1477059 : _vel_max_ne_ms = fabsf(speed_ne_ms);
461 1477059 : _accel_max_ne_mss = fabsf(accel_ne_mss);
462 :
463 : // ensure the horizontal jerk is less than the vehicle is capable of
464 1477059 : const float jerk_max_msss = MIN(_attitude_control.get_ang_vel_roll_max_rads(), _attitude_control.get_ang_vel_pitch_max_rads()) * GRAVITY_MSS;
465 1477059 : const float snap_max_mssss = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS;
466 :
467 : // get specified jerk limit
468 1477059 : _jerk_max_ne_msss = _shaping_jerk_ne_msss;
469 :
470 : // limit maximum jerk based on maximum angular rate
471 1477059 : if (is_positive(jerk_max_msss) && _attitude_control.get_bf_feedforward()) {
472 1295480 : _jerk_max_ne_msss = MIN(_jerk_max_ne_msss, jerk_max_msss);
473 : }
474 :
475 : // limit maximum jerk to maximum possible average jerk based on angular acceleration
476 1477059 : if (is_positive(snap_max_mssss) && _attitude_control.get_bf_feedforward()) {
477 1477059 : _jerk_max_ne_msss = MIN(0.5 * safe_sqrt(_accel_max_ne_mss * snap_max_mssss), _jerk_max_ne_msss);
478 : }
479 1477059 : }
480 :
481 : // Sets horizontal correction limits for velocity (cm/s) and acceleration (cm/s²).
482 : // Should be called only during initialization to avoid control discontinuities.
483 : // All arguments should be positive.
484 : // See NE_set_correction_speed_accel_m() for full details.
485 16 : void AC_PosControl::NE_set_correction_speed_accel_cm(float speed_ne_cms, float accel_ne_cmss)
486 : {
487 16 : NE_set_correction_speed_accel_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01);
488 16 : }
489 :
490 : // Sets horizontal correction limits for velocity (m/s) and acceleration (m/s²).
491 : // These values constrain the PID correction path, not the desired trajectory.
492 : // All arguments should be positive.
493 2738914 : void AC_PosControl::NE_set_correction_speed_accel_m(float speed_ne_ms, float accel_ne_mss)
494 : {
495 : // limits that are not positive are ignored
496 2738914 : _p_pos_ne_m.set_limits(speed_ne_ms, accel_ne_mss, 0.0f);
497 2738914 : }
498 :
499 : // Initializes NE controller to a stationary stopping point with zero velocity and acceleration.
500 : // Use when the expected trajectory begins at rest but the starting position is unspecified.
501 : // The starting position can be retrieved with get_pos_target_NED_m().
502 23534 : void AC_PosControl::NE_init_controller_stopping_point()
503 : {
504 23534 : NE_init_controller();
505 :
506 23534 : get_stopping_point_NE_m(_pos_desired_ned_m.xy());
507 23534 : _pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy();
508 23534 : _vel_desired_ned_ms.xy().zero();
509 23534 : _accel_desired_ned_mss.xy().zero();
510 23534 : }
511 :
512 : // Smoothly decays NE acceleration over time to zero while maintaining current velocity and position.
513 : // Reduces output acceleration by ~95% over 0.5 seconds to avoid abrupt transitions.
514 3682278 : void AC_PosControl::NE_relax_velocity_controller()
515 : {
516 : // decay acceleration and therefore current attitude target to zero
517 : // this will be reset by NE_init_controller() if !NE_is_active()
518 3682278 : if (is_positive(_dt_s)) {
519 3590707 : float decay = 1.0 - _dt_s / (_dt_s + POSCONTROL_RELAX_TC);
520 3590707 : _accel_target_ned_mss.xy() *= decay;
521 : }
522 :
523 3682278 : NE_init_controller();
524 3682278 : }
525 :
526 : // Softens NE controller for landing by reducing position error and suppressing I-term windup.
527 : // Used to make descent behavior more stable near ground contact.
528 1534563 : void AC_PosControl::NE_soften_for_landing()
529 : {
530 : // decay position error to zero
531 1534563 : if (is_positive(_dt_s)) {
532 1534563 : _pos_target_ned_m.xy() += (_pos_estimate_ned_m.xy() - _pos_target_ned_m.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC));
533 1534563 : _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
534 : }
535 :
536 : // Prevent I term build up in xy velocity controller.
537 : // Note that this flag is reset on each loop in NE_update_controller()
538 1534563 : NE_set_externally_limited();
539 1534563 : }
540 :
541 : // Fully initializes the NE controller with current position, velocity, acceleration, and attitude.
542 : // Intended for normal startup when the full state is known.
543 : // Private function shared by other NE initializers.
544 3708875 : void AC_PosControl::NE_init_controller()
545 : {
546 : // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
547 3708875 : NE_init_offsets();
548 :
549 : // set roll, pitch lean angle targets to current attitude
550 3708875 : const Vector3f &att_target_euler_rad = _attitude_control.get_att_target_euler_rad();
551 3708875 : _roll_target_rad = att_target_euler_rad.x;
552 3708875 : _pitch_target_rad = att_target_euler_rad.y;
553 3708875 : _yaw_target_rad = att_target_euler_rad.z; // todo: this should be thrust vector heading, not yaw.
554 3708875 : _yaw_rate_target_rads = 0.0f;
555 3708875 : _angle_max_override_rad = 0.0;
556 :
557 3708875 : _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
558 3708875 : _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
559 :
560 3708875 : _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy();
561 3708875 : _vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy();
562 :
563 : // Set desired acceleration to zero because raw acceleration is prone to noise
564 3708875 : _accel_desired_ned_mss.xy().zero();
565 :
566 3708875 : if (!NE_is_active()) {
567 965 : lean_angles_to_accel_NE_mss(_accel_target_ned_mss.x, _accel_target_ned_mss.y);
568 : }
569 :
570 : // limit acceleration using maximum lean angles
571 3708875 : const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
572 3708875 : const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
573 3708875 : _accel_target_ned_mss.xy().limit_length(accel_max_mss);
574 :
575 : // initialise I terms from lean angles
576 3708875 : _pid_vel_ne_m.reset_filter();
577 : // initialise the I term to (_accel_target_ned_mss - _accel_desired_ned_mss)
578 : // _accel_desired_ned_mss is zero and can be removed from the equation
579 3708875 : _pid_vel_ne_m.set_integrator((_accel_target_ned_mss.xy() - _vel_target_ned_ms.xy() * _pid_vel_ne_m.ff()));
580 :
581 : // initialise ekf xy reset handler
582 3708875 : NE_init_ekf_reset();
583 :
584 : // initialise z_controller time out
585 3708875 : _last_update_ne_ticks = AP::scheduler().ticks32();
586 3708875 : }
587 :
588 : // Sets the desired NE-plane acceleration in m/s² using jerk-limited shaping.
589 : // Smoothly transitions to the specified acceleration from current kinematic state.
590 : // Constraints: max acceleration and jerk set via NE_set_max_speed_accel_m().
591 2475 : void AC_PosControl::input_accel_NE_m(const Vector2f& accel_ne_mss)
592 : {
593 2475 : update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
594 2475 : shape_accel_xy(accel_ne_mss, _accel_desired_ned_mss.xy(), _jerk_max_ne_msss, _dt_s);
595 2475 : }
596 :
597 : // Sets desired NE-plane velocity and acceleration (cm/s, cm/s²) using jerk-limited shaping.
598 : // See input_vel_accel_NE_m() for full details.
599 73639 : void AC_PosControl::input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output)
600 : {
601 73639 : Vector2f vel_ne_ms = vel_ne_cms * 0.01;
602 73639 : input_vel_accel_NE_m(vel_ne_ms, accel_ne_cmss * 0.01, limit_output);
603 73639 : vel_ne_cms = vel_ne_ms * 100.0;
604 73639 : }
605 :
606 : // Sets desired NE-plane velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
607 : // Calculates target acceleration using current kinematics constrained by acceleration and jerk limits.
608 : // If `limit_output` is true, applies limits to total command (desired + correction).
609 7985561 : void AC_PosControl::input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output)
610 : {
611 7985561 : update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
612 :
613 7985561 : shape_vel_accel_xy(vel_ne_ms, accel_ne_mss, _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
614 : _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output);
615 :
616 7985561 : update_vel_accel_xy(vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f());
617 7985561 : }
618 :
619 : // Sets desired NE position, velocity, and acceleration (cm, cm/s, cm/s²) with jerk-limited shaping.
620 : // See input_pos_vel_accel_NE_m() for full details.
621 73466 : void AC_PosControl::input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output)
622 : {
623 73466 : Vector2p pos_ne_m = pos_ne_cm * 0.01;
624 73466 : Vector2f vel_ne_ms = vel_ne_cms * 0.01;
625 73466 : input_pos_vel_accel_NE_m(pos_ne_m, vel_ne_ms, accel_ne_cmss * 0.01, limit_output);
626 73466 : pos_ne_cm = pos_ne_m * 100.0;
627 73466 : vel_ne_cms = vel_ne_ms * 100.0;
628 73466 : }
629 :
630 : // Sets desired NE position, velocity, and acceleration (m, m/s, m/s²) with jerk-limited shaping.
631 : // Calculates acceleration trajectory based on current kinematics and constraints.
632 : // If `limit_output` is true, limits apply to full command (desired + correction).
633 1196039 : void AC_PosControl::input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output)
634 : {
635 1196039 : update_pos_vel_accel_xy(_pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
636 :
637 1196039 : shape_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _pos_desired_ned_m.xy(), _vel_desired_ned_ms.xy(), _accel_desired_ned_mss.xy(),
638 : _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output);
639 :
640 1196039 : update_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f(), Vector2f());
641 1196039 : }
642 :
643 : // Updates NE offsets by gradually moving them toward their targets.
644 19264067 : void AC_PosControl::NE_update_offsets()
645 : {
646 : // Check if NE offset targets have timed out
647 19264067 : uint32_t now_ms = AP_HAL::millis();
648 19264067 : if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
649 : // Timeout: reset all NE offset targets to zero
650 19226218 : _pos_offset_target_ned_m.xy().zero();
651 19226218 : _vel_offset_target_ned_ms.xy().zero();
652 19226218 : _accel_offset_target_ned_mss.xy().zero();
653 : }
654 :
655 : // Advance offset target kinematic state (position, velocity, accel)
656 19264067 : update_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f());
657 19264067 : update_pos_vel_accel_xy(_pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(), _dt_s, _limit_vector_ned.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_m.get_error());
658 :
659 : // Shape the offset path from current to target using jerk-limited smoothing
660 19264067 : shape_pos_vel_accel_xy(_pos_offset_target_ned_m.xy(), _vel_offset_target_ned_ms.xy(), _accel_offset_target_ned_mss.xy(),
661 19264067 : _pos_offset_ned_m.xy(), _vel_offset_ned_ms.xy(), _accel_offset_ned_mss.xy(),
662 : _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false);
663 19264067 : }
664 :
665 : // Disables NE position correction by setting the target position to the current position.
666 : // Useful to freeze positional control without disrupting velocity control.
667 217209 : void AC_PosControl::NE_stop_pos_stabilisation()
668 : {
669 217209 : _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
670 217209 : _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
671 217209 : }
672 :
673 : // Disables NE position and velocity correction by setting target values to current state.
674 : // Useful to prevent further corrections and freeze motion stabilization in NE axes.
675 0 : void AC_PosControl::NE_stop_vel_stabilisation()
676 : {
677 0 : _pos_target_ned_m.xy() = _pos_estimate_ned_m.xy();
678 0 : _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
679 :
680 0 : _vel_target_ned_ms.xy() = _vel_estimate_ned_ms.xy();
681 0 : _vel_desired_ned_ms.xy() = _vel_target_ned_ms.xy() - _vel_offset_ned_ms.xy();
682 :
683 : // reset I terms
684 0 : _pid_vel_ne_m.reset_filter();
685 0 : _pid_vel_ne_m.reset_I();
686 0 : }
687 :
688 : // Returns true if the NE position controller has run in the last 5 control loop cycles.
689 33749256 : bool AC_PosControl::NE_is_active() const
690 : {
691 33749256 : const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_ne_ticks;
692 33749256 : return dt_ticks <= 1;
693 : }
694 :
695 : // Uses P and PID controllers to generate corrections which are added to feedforward velocity/acceleration.
696 : // Requires all desired targets to be pre-set using the input_* or set_* methods.
697 19264067 : void AC_PosControl::NE_update_controller()
698 : {
699 : // check for ekf xy position reset
700 19264067 : NE_handle_ekf_reset();
701 :
702 : // Check for position control time out
703 19264067 : if (!NE_is_active()) {
704 0 : NE_init_controller();
705 0 : if (has_good_timing()) {
706 : // call internal error because initialisation has not been done
707 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
708 : }
709 : }
710 19264067 : _last_update_ne_ticks = AP::scheduler().ticks32();
711 :
712 : // the estimator might require scaling down of control
713 : // (e.g. because of sensors being used to arrive at attitude
714 : // estimate):
715 19264067 : const float ahrsControlScaleXY = AP::ahrs().get_control_gain_scaler_XY();
716 :
717 : // Update lateral position, velocity, and acceleration offsets using path shaping
718 19264067 : NE_update_offsets();
719 :
720 : // Position Controller
721 :
722 : // Combine position target with active NE offset to get absolute target
723 19264067 : _pos_target_ned_m.xy() = _pos_desired_ned_m.xy() + _pos_offset_ned_m.xy();
724 :
725 : // determine the combined position of the actual position and the disturbance from system ID mode
726 : // calculate the target velocity correction
727 19264067 : Vector2p comb_pos_ne_m = _pos_estimate_ned_m.xy();
728 19264067 : comb_pos_ne_m += _disturb_pos_ne_m.topostype();
729 :
730 : // Run P controller to compute velocity setpoint from position error
731 19264067 : Vector2f vel_target_ne_ms = _p_pos_ne_m.update_all(_pos_target_ned_m.xy(), comb_pos_ne_m);
732 19264067 : _pos_desired_ned_m.xy() = _pos_target_ned_m.xy() - _pos_offset_ned_m.xy();
733 :
734 : // Velocity Controller
735 :
736 : // Apply AHRS scaling (e.g. for optical flow noise compensation)
737 19264067 : vel_target_ne_ms *= ahrsControlScaleXY;
738 19264067 : vel_target_ne_ms *= _ne_control_scale_factor;
739 :
740 19264067 : _vel_target_ned_ms.xy() = vel_target_ne_ms;
741 19264067 : _vel_target_ned_ms.xy() += _vel_desired_ned_ms.xy() + _vel_offset_ned_ms.xy();
742 :
743 : // Velocity Controller
744 :
745 : // determine the combined velocity of the actual velocity and the disturbance from system ID mode
746 19264067 : Vector2f comb_vel_ne_ms = _vel_estimate_ned_ms.xy();
747 19264067 : comb_vel_ne_ms += _disturb_vel_ne_ms;
748 :
749 : // Run velocity PID controller and scale result for control authority
750 19264067 : Vector2f accel_target_ne_mss = _pid_vel_ne_m.update_all(_vel_target_ned_ms.xy(), comb_vel_ne_ms, _dt_s, _limit_vector_ned.xy());
751 :
752 : // Acceleration Controller
753 :
754 : // Apply AHRS scaling again to correct for measurement distortions
755 19264067 : accel_target_ne_mss *= ahrsControlScaleXY;
756 19264067 : accel_target_ne_mss *= _ne_control_scale_factor;
757 :
758 19264067 : _ne_control_scale_factor = 1.0;
759 :
760 : // pass the correction acceleration to the target acceleration output
761 19264067 : _accel_target_ned_mss.xy() = accel_target_ne_mss;
762 19264067 : _accel_target_ned_mss.xy() += _accel_desired_ned_mss.xy() + _accel_offset_ned_mss.xy();
763 :
764 : // limit acceleration using maximum lean angles
765 19264067 : const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
766 19264067 : const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
767 : // Save unbounded target for use in "limited" check (not unit-consistent with z!)
768 19264067 : _limit_vector_ned.xy() = _accel_target_ned_mss.xy();
769 : // Normalise desired velocity by max speed for the cross-track reference (guard zero max speed).
770 19264067 : Vector2f vel_norm_ne;
771 19264067 : if (is_positive(_vel_max_ne_ms)) {
772 19264067 : vel_norm_ne = _vel_desired_ned_ms.xy() / _vel_max_ne_ms;
773 : }
774 19264067 : if (!limit_accel_xy(vel_norm_ne, _accel_target_ned_mss.xy(), accel_max_mss)) {
775 : // _accel_target_ned_mss was not limited so we can zero the xy limit vector
776 18927028 : _limit_vector_ned.xy().zero();
777 : }
778 :
779 : // Convert acceleration to roll/pitch angle targets (used by attitude controller)
780 19264067 : accel_NE_mss_to_lean_angles_rad(_accel_target_ned_mss.x, _accel_target_ned_mss.y, _roll_target_rad, _pitch_target_rad);
781 :
782 : // Update yaw and yaw rate targets to match heading of motion
783 19264067 : calculate_yaw_and_rate_yaw();
784 :
785 : // reset the disturbance from system ID mode to zero
786 19264067 : _disturb_pos_ne_m.zero();
787 19264067 : _disturb_vel_ne_ms.zero();
788 19264067 : }
789 :
790 :
791 : ///
792 : /// Vertical position controller
793 : ///
794 :
795 : // Sets maximum climb/descent rate (cm/s) and vertical acceleration (cm/s²) for the U-axis.
796 : // See D_set_max_speed_accel_m() for full details.
797 : // All values must be positive.
798 291080 : void AC_PosControl::D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss)
799 : {
800 291080 : D_set_max_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01);
801 291080 : }
802 :
803 : // Sets maximum climb/descent rate (m/s) and vertical acceleration (m/s²) for the U-axis.
804 : // These values are used for jerk-limited kinematic shaping of the vertical trajectory.
805 : // All values must be positive.
806 10923735 : void AC_PosControl::D_set_max_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss)
807 : {
808 : // sanity check and update
809 10923735 : if (!is_zero(decent_speed_max_ms)) {
810 10923735 : _vel_max_down_ms = fabsf(decent_speed_max_ms);
811 : }
812 10923735 : if (!is_zero(climb_speed_max_ms)) {
813 10923735 : _vel_max_up_ms = fabsf(climb_speed_max_ms);
814 : }
815 10923735 : if (!is_zero(accel_max_d_mss)) {
816 10923735 : _accel_max_d_mss = fabsf(accel_max_d_mss);
817 : }
818 :
819 : // ensure the vertical Jerk is not limited by the filters in the Z acceleration PID object
820 10923735 : _jerk_max_d_msss = _shaping_jerk_d_msss;
821 10923735 : if (is_positive(_pid_accel_d_m.filt_T_hz())) {
822 62333 : _jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_T_hz()) / 5.0);
823 : }
824 10923735 : if (is_positive(_pid_accel_d_m.filt_E_hz())) {
825 10875566 : _jerk_max_d_msss = MIN(_jerk_max_d_msss, MIN(GRAVITY_MSS, _accel_max_d_mss) * (M_2PI * _pid_accel_d_m.filt_E_hz()) / 5.0);
826 : }
827 10923735 : }
828 :
829 : // Sets vertical correction velocity and acceleration limits (cm/s, cm/s²).
830 : // Should only be called during initialization to avoid discontinuities.
831 : // See set_correction_speed_accel_U_m() for full details.
832 : // All values must be positive.
833 40 : void AC_PosControl::D_set_correction_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss)
834 : {
835 40 : D_set_correction_speed_accel_m(decent_speed_max_cms * 0.01, climb_speed_max_cms * 0.01, accel_max_u_cmss * 0.01);
836 40 : }
837 :
838 : // Sets vertical correction velocity and acceleration limits (m/s, m/s²).
839 : // These values constrain the correction output of the PID controller.
840 : // All values must be positive.
841 3219594 : void AC_PosControl::D_set_correction_speed_accel_m(float decent_speed_max_ms, float climb_speed_max_ms, float accel_max_d_mss)
842 : {
843 : // define maximum position error and maximum first and second differential limits
844 3219594 : _p_pos_d_m.set_limits(-fabsf(decent_speed_max_ms), fabsf(climb_speed_max_ms), fabsf(accel_max_d_mss), 0.0f);
845 3219594 : }
846 :
847 : // Initializes U-axis controller to current position, velocity, and acceleration, disallowing descent.
848 : // Used for takeoff or hold scenarios where downward motion is prohibited.
849 12 : void AC_PosControl::D_init_controller_no_descent()
850 : {
851 : // Initialise the position controller to the current throttle, position, velocity and acceleration.
852 12 : D_init_controller();
853 :
854 : // remove all descent if present
855 12 : _vel_target_ned_ms.z = MIN(0.0, _vel_target_ned_ms.z);
856 12 : _vel_desired_ned_ms.z = MIN(0.0, _vel_desired_ned_ms.z);
857 12 : _vel_terrain_d_ms = MIN(0.0, _vel_terrain_d_ms);
858 12 : _vel_offset_ned_ms.z = MIN(0.0, _vel_offset_ned_ms.z);
859 12 : _accel_target_ned_mss.z = MIN(0.0, _accel_target_ned_mss.z);
860 12 : _accel_desired_ned_mss.z = MIN(0.0, _accel_desired_ned_mss.z);
861 12 : _accel_terrain_d_mss = MIN(0.0, _accel_terrain_d_mss);
862 12 : _accel_offset_ned_mss.z = MIN(0.0, _accel_offset_ned_mss.z);
863 12 : }
864 :
865 : // Initializes U-axis controller to a stationary stopping point with zero velocity and acceleration.
866 : // Used when the trajectory starts at rest but the initial altitude is unspecified.
867 : // The resulting position target can be retrieved with get_pos_target_NED_m().
868 17456 : void AC_PosControl::D_init_controller_stopping_point()
869 : {
870 : // Initialise the position controller to the current throttle, position, velocity and acceleration.
871 17456 : D_init_controller();
872 :
873 17456 : get_stopping_point_D_m(_pos_desired_ned_m.z);
874 17456 : _pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z;
875 17456 : _vel_desired_ned_ms.z = 0.0f;
876 17456 : _accel_desired_ned_mss.z = 0.0f;
877 17456 : }
878 :
879 : // Smoothly decays U-axis acceleration to zero over time while maintaining current vertical velocity.
880 : // Reduces requested acceleration by ~95% every 0.5 seconds to avoid abrupt transitions.
881 : // `throttle_setting` is used to determine whether to preserve positive acceleration in low-thrust cases.
882 6019125 : void AC_PosControl::D_relax_controller(float throttle_setting)
883 : {
884 : // Initialise the position controller to the current position, velocity and acceleration.
885 6019125 : D_init_controller();
886 :
887 : // D_init_controller has set the acceleration PID I term to generate the current throttle set point
888 : // Use relax_integrator to decay the throttle set point to throttle_setting
889 6019125 : _pid_accel_d_m.relax_integrator(-(throttle_setting - _motors.get_throttle_hover()), _dt_s, POSCONTROL_RELAX_TC);
890 6019125 : }
891 :
892 : // Fully initializes the U-axis controller with current position, velocity, acceleration, and attitude.
893 : // Used during standard controller activation when full state is known.
894 : // Private function shared by other vertical initializers.
895 6184169 : void AC_PosControl::D_init_controller()
896 : {
897 : // initialise terrain targets and offsets to zero
898 6184169 : init_terrain();
899 :
900 : // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
901 6184169 : D_init_offsets();
902 :
903 6184169 : _pos_target_ned_m.z = _pos_estimate_ned_m.z;
904 6184169 : _pos_desired_ned_m.z = _pos_target_ned_m.z - _pos_offset_ned_m.z;
905 :
906 6184169 : _vel_target_ned_ms.z = _vel_estimate_ned_ms.z;
907 6184169 : _vel_desired_ned_ms.z = _vel_target_ned_ms.z - _vel_offset_ned_ms.z;
908 :
909 : // Reset I term of velocity PID
910 6184169 : _pid_vel_d_m.reset_filter();
911 6184169 : _pid_vel_d_m.set_integrator(0.0f);
912 :
913 6184169 : _accel_target_ned_mss.z = constrain_float(get_estimated_accel_D_mss(), -_accel_max_d_mss, _accel_max_d_mss);
914 6184169 : _accel_desired_ned_mss.z = _accel_target_ned_mss.z - (_accel_offset_ned_mss.z + _accel_terrain_d_mss);
915 6184169 : _pid_accel_d_m.reset_filter();
916 :
917 : // Set acceleration PID I term based on the current throttle
918 : // Remove the expected P term due to _accel_desired_ned_mss.z being constrained to _accel_max_d_mss
919 : // Remove the expected FF term due to non-zero _accel_target_ned_mss.z
920 6184169 : _pid_accel_d_m.set_integrator(-(_attitude_control.get_throttle_in() - _motors.get_throttle_hover())
921 6184169 : - _pid_accel_d_m.kP() * (_accel_target_ned_mss.z - get_estimated_accel_D_mss())
922 6184169 : - _pid_accel_d_m.ff() * _accel_target_ned_mss.z);
923 :
924 : // initialise ekf z reset handler
925 6184169 : D_init_ekf_reset();
926 :
927 : // initialise z_controller time out
928 6184169 : _last_update_d_ticks = AP::scheduler().ticks32();
929 6184169 : }
930 :
931 : // Sets the desired vertical acceleration in m/s² using jerk-limited shaping.
932 : // Smoothly transitions to the target acceleration from current kinematic state.
933 : // Constraints: max acceleration and jerk set via D_set_max_speed_accel_m().
934 2475 : void AC_PosControl::input_accel_D_m(float accel_d_mss)
935 : {
936 : // calculated increased maximum jerk if over speed
937 2475 : float jerk_max_d_msss = _jerk_max_d_msss * calculate_overspeed_gain();
938 :
939 : // adjust desired alt if motors have not hit their limits
940 2475 : update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
941 :
942 2475 : shape_accel(accel_d_mss, _accel_desired_ned_mss.z, jerk_max_d_msss, _dt_s);
943 2475 : }
944 :
945 : // Sets desired vertical velocity and acceleration (m/s, m/s²) using jerk-limited shaping.
946 : // Calculates required acceleration using current vertical kinematics.
947 : // If `limit_output` is true, limits apply to the combined (desired + correction) command.
948 13666457 : void AC_PosControl::input_vel_accel_D_m(float &vel_d_ms, float accel_d_mss, bool limit_output)
949 : {
950 : // calculated increased maximum acceleration and jerk if over speed
951 13666457 : const float overspeed_gain = calculate_overspeed_gain();
952 13666457 : const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
953 13666457 : const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
954 :
955 : // adjust desired alt if motors have not hit their limits
956 13666457 : update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
957 :
958 13666457 : shape_vel_accel(vel_d_ms, accel_d_mss,
959 13666457 : _vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
960 : -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
961 : jerk_max_d_msss, _dt_s, limit_output);
962 :
963 13666457 : update_vel_accel(vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0);
964 13666457 : }
965 :
966 : // Generates a vertical trajectory using the given climb rate in cm/s and jerk-limited shaping.
967 : // Adjusts the internal target altitude based on integrated climb rate.
968 : // See set_pos_target_D_from_climb_rate_m() for full details.
969 370753 : void AC_PosControl::D_set_pos_target_from_climb_rate_cms(float climb_rate_cms)
970 : {
971 370753 : D_set_pos_target_from_climb_rate_ms(climb_rate_cms * 0.01);
972 370753 : }
973 :
974 : // Generates a vertical trajectory using the given climb rate in m/s and jerk-limited shaping.
975 : // Target altitude is updated over time by integrating the climb rate.
976 12280047 : void AC_PosControl::D_set_pos_target_from_climb_rate_ms(float climb_rate_ms, bool ignore_descent_limit)
977 : {
978 12280047 : if (ignore_descent_limit) {
979 : // turn off limits in the down (positive z) direction
980 3922277 : _limit_vector_ned.z = MIN(_limit_vector_ned.z, 0.0f);
981 : }
982 :
983 12280047 : float vel_d_ms = -climb_rate_ms;
984 12280047 : input_vel_accel_D_m(vel_d_ms, 0.0);
985 12280047 : }
986 :
987 : // Sets vertical position, velocity, and acceleration in cm using jerk-limited shaping.
988 : // See input_pos_vel_accel_D_m() for full details.
989 73466 : void AC_PosControl::input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output)
990 : {
991 73466 : float pos_d_m = -pos_u_cm * 0.01;
992 73466 : float vel_d_ms = -vel_u_cms * 0.01;
993 73466 : const float accel_d_mss = -accel_u_cmss * 0.01;
994 73466 : input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, accel_d_mss, limit_output);
995 73466 : pos_u_cm = -pos_d_m * 100.0;
996 73466 : vel_u_cms = -vel_d_ms * 100.0;
997 73466 : }
998 :
999 : // Sets vertical position, velocity, and acceleration in meters using jerk-limited shaping.
1000 : // Calculates required acceleration using current state and constraints.
1001 : // If `limit_output` is true, limits are applied to combined (desired + correction) command.
1002 3201940 : void AC_PosControl::input_pos_vel_accel_D_m(float &pos_d_m, float &vel_d_ms, float accel_d_mss, bool limit_output)
1003 : {
1004 : // calculated increased maximum acceleration and jerk if over speed
1005 3201940 : const float overspeed_gain = calculate_overspeed_gain();
1006 3201940 : const float accel_max_d_mss = _accel_max_d_mss * overspeed_gain;
1007 3201940 : const float jerk_max_d_msss = _jerk_max_d_msss * overspeed_gain;
1008 :
1009 : // adjust desired altitude if motors have not hit their limits
1010 3201940 : update_pos_vel_accel(_pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z, _dt_s, _limit_vector_ned.z, _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
1011 :
1012 3201940 : shape_pos_vel_accel(pos_d_m, vel_d_ms, accel_d_mss,
1013 3201940 : _pos_desired_ned_m.z, _vel_desired_ned_ms.z, _accel_desired_ned_mss.z,
1014 3201940 : -_vel_max_up_ms, _vel_max_down_ms,
1015 : -accel_max_d_mss, constrain_float(accel_max_d_mss, 0.0, 7.5),
1016 : jerk_max_d_msss, _dt_s, limit_output);
1017 :
1018 3201940 : postype_t posp = pos_d_m;
1019 3201940 : update_pos_vel_accel(posp, vel_d_ms, accel_d_mss, _dt_s, 0.0, 0.0, 0.0);
1020 3201940 : pos_d_m = posp;
1021 3201940 : }
1022 :
1023 : // Sets target altitude in cm using jerk-limited shaping to gradually move to the new position.
1024 : // See D_set_alt_target_with_slew_m() for full details.
1025 0 : void AC_PosControl::set_alt_target_with_slew_cm(float pos_u_cm)
1026 : {
1027 0 : D_set_alt_target_with_slew_m(pos_u_cm * 0.01);
1028 0 : }
1029 :
1030 : // Sets target altitude in meters using jerk-limited shaping.
1031 18452 : void AC_PosControl::D_set_alt_target_with_slew_m(float pos_u_m)
1032 : {
1033 18452 : float pos_d_m = -pos_u_m;
1034 18452 : float zero = 0;
1035 18452 : input_pos_vel_accel_D_m(pos_d_m, zero, 0);
1036 18452 : }
1037 :
1038 : // Updates vertical (U) offsets by gradually moving them toward their targets.
1039 25567102 : void AC_PosControl::D_update_offsets()
1040 : {
1041 : // Check if vertical offset targets have timed out
1042 25567102 : uint32_t now_ms = AP_HAL::millis();
1043 25567102 : if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
1044 : // Timeout: reset U-axis offset targets to zero
1045 25529253 : _pos_offset_target_ned_m.z = 0.0;
1046 25529253 : _vel_offset_target_ned_ms.z = 0.0;
1047 25529253 : _accel_offset_target_ned_mss.z = 0.0;
1048 : }
1049 :
1050 : // Advance current offset state using PID-derived feedback and vertical limits
1051 25567102 : postype_t p_offset_d_m = _pos_offset_ned_m.z;
1052 25567102 : update_pos_vel_accel(p_offset_d_m, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
1053 25567102 : _pos_offset_ned_m.z = p_offset_d_m;
1054 :
1055 : // Shape offset trajectory (position/velocity/acceleration) using jerk-limited smoothing
1056 51134204 : shape_pos_vel_accel(_pos_offset_target_ned_m.z, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z,
1057 25567102 : _pos_offset_ned_m.z, _vel_offset_ned_ms.z, _accel_offset_ned_mss.z,
1058 25567102 : -get_max_speed_up_ms(), get_max_speed_down_ms(),
1059 25567102 : -D_get_max_accel_mss(), D_get_max_accel_mss(),
1060 : _jerk_max_d_msss, _dt_s, false);
1061 :
1062 : // Update target state forward in time with assumed zero velocity/acceleration targets
1063 25567102 : p_offset_d_m = _pos_offset_target_ned_m.z;
1064 25567102 : update_pos_vel_accel(p_offset_d_m, _vel_offset_target_ned_ms.z, _accel_offset_target_ned_mss.z, _dt_s, 0.0, 0.0, 0.0);
1065 25567102 : _pos_offset_target_ned_m.z = p_offset_d_m;
1066 25567102 : }
1067 :
1068 : // Returns true if the U-axis controller has run in the last 5 control loop cycles.
1069 31509876 : bool AC_PosControl::D_is_active() const
1070 : {
1071 31509876 : const uint32_t dt_ticks = AP::scheduler().ticks32() - _last_update_d_ticks;
1072 31509876 : return dt_ticks <= 1;
1073 : }
1074 :
1075 : // Runs the vertical (U-axis) position controller.
1076 : // Computes output acceleration based on position and velocity errors using PID correction.
1077 : // Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command.
1078 : // Desired position, velocity, and acceleration must be set before calling.
1079 25567102 : void AC_PosControl::D_update_controller()
1080 : {
1081 : // check for ekf z-axis position reset
1082 25567102 : D_handle_ekf_reset();
1083 :
1084 : // Check for z_controller time out
1085 25567102 : if (!D_is_active()) {
1086 0 : D_init_controller();
1087 0 : if (has_good_timing()) {
1088 : // call internal error because initialisation has not been done
1089 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1090 : }
1091 : }
1092 25567102 : _last_update_d_ticks = AP::scheduler().ticks32();
1093 :
1094 : // Update vertical offset targets and terrain estimate
1095 25567102 : D_update_offsets();
1096 25567102 : update_terrain();
1097 :
1098 : // Position Controller
1099 :
1100 : // Combine desired + offset + terrain for final position target
1101 25567102 : _pos_target_ned_m.z = _pos_desired_ned_m.z + _pos_offset_ned_m.z + _pos_terrain_d_m;
1102 :
1103 : // P controller: convert position error to velocity target
1104 25567102 : _vel_target_ned_ms.z = _p_pos_d_m.update_all(_pos_target_ned_m.z, _pos_estimate_ned_m.z);
1105 25567102 : _vel_target_ned_ms.z *= AP::ahrs().get_control_gain_scaler_Z();
1106 :
1107 25567102 : _pos_desired_ned_m.z = _pos_target_ned_m.z - (_pos_offset_ned_m.z + _pos_terrain_d_m);
1108 :
1109 : // add feed forward component
1110 25567102 : _vel_target_ned_ms.z += _vel_desired_ned_ms.z + _vel_offset_ned_ms.z + _vel_terrain_d_ms;
1111 :
1112 : // Velocity Controller
1113 :
1114 : // PID controller: convert velocity error to acceleration
1115 25567102 : _accel_target_ned_mss.z = _pid_vel_d_m.update_all(_vel_target_ned_ms.z, _vel_estimate_ned_ms.z, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
1116 25567102 : _accel_target_ned_mss.z *= AP::ahrs().get_control_gain_scaler_Z();
1117 :
1118 : // add feed forward component
1119 25567102 : _accel_target_ned_mss.z += _accel_desired_ned_mss.z + _accel_offset_ned_mss.z + _accel_terrain_d_mss;
1120 :
1121 : // Acceleration Controller
1122 :
1123 : // Gravity-compensated vertical acceleration measurement (positive = up)
1124 25567102 : const float estimated_accel_d_mss = get_estimated_accel_D_mss();
1125 :
1126 : // Ensure integrator can produce enough thrust to overcome hover throttle
1127 25567102 : if (_motors.get_throttle_hover() > _pid_accel_d_m.imax()) {
1128 16 : _pid_accel_d_m.set_imax(_motors.get_throttle_hover());
1129 : }
1130 : float thrust_d_norm;
1131 25567102 : if (_vibe_comp_enabled) {
1132 : // Use vibration-resistant throttle estimator (feedforward + scaled integrator)
1133 11757 : thrust_d_norm = get_throttle_with_vibration_override();
1134 : } else {
1135 : // Standard PID update using vertical acceleration error
1136 25555345 : thrust_d_norm = _pid_accel_d_m.update_all(_accel_target_ned_mss.z, estimated_accel_d_mss, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper));
1137 : // Include FF contribution to reduce delay
1138 25555345 : thrust_d_norm += _pid_accel_d_m.get_ff();
1139 : }
1140 25567102 : thrust_d_norm -= _motors.get_throttle_hover();
1141 :
1142 : // Actuator commands
1143 :
1144 : // Send final throttle output to attitude controller (includes angle boost)
1145 25567102 : _attitude_control.set_throttle_out(-thrust_d_norm, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ);
1146 :
1147 : // Check for vertical controller health
1148 :
1149 : // Update health indicator based on error magnitude vs configured speed range
1150 25567102 : float error_ratio = _pid_vel_d_m.get_error() / _vel_max_down_ms;
1151 25567102 : _vel_d_control_ratio += _dt_s * 0.1f * (0.5 - error_ratio);
1152 25567102 : _vel_d_control_ratio = constrain_float(_vel_d_control_ratio, 0.0f, 2.0f);
1153 :
1154 : // set vertical component of the limit vector
1155 25567102 : if (_motors.limit.throttle_upper) {
1156 3665343 : _limit_vector_ned.z = -1.0f;
1157 21901759 : } else if (_motors.limit.throttle_lower) {
1158 1023158 : _limit_vector_ned.z = 1.0f;
1159 : } else {
1160 20878601 : _limit_vector_ned.z = 0.0f;
1161 : }
1162 25567102 : }
1163 :
1164 :
1165 : ///
1166 : /// Accessors
1167 : ///
1168 :
1169 : // Returns the maximum allowed roll/pitch angle in radians.
1170 30171717 : float AC_PosControl::get_lean_angle_max_rad() const
1171 : {
1172 30171717 : if (is_positive(_angle_max_override_rad)) {
1173 1341938 : return _angle_max_override_rad;
1174 : }
1175 28829779 : if (!is_positive(_lean_angle_max_deg)) {
1176 28646005 : return _attitude_control.lean_angle_max_rad();
1177 : }
1178 183774 : return radians(_lean_angle_max_deg);
1179 : }
1180 :
1181 : // Sets externally computed NED position, velocity, and acceleration in meters, m/s, and m/s².
1182 : // Use when path planning or shaping is done outside this controller.
1183 4009049 : void AC_PosControl::set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss)
1184 : {
1185 4009049 : _pos_desired_ned_m = pos_ned_m;
1186 4009049 : _vel_desired_ned_ms = vel_ned_ms;
1187 4009049 : _accel_desired_ned_mss = accel_ned_mss;
1188 4009049 : }
1189 :
1190 : // Sets externally computed NE position, velocity, and acceleration in meters, m/s, and m/s².
1191 : // Use when path planning or shaping is done outside this controller.
1192 3029099 : void AC_PosControl::set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss)
1193 : {
1194 3029099 : _pos_desired_ned_m.xy() = pos_ne_m;
1195 3029099 : _vel_desired_ned_ms.xy() = vel_ne_ms;
1196 3029099 : _accel_desired_ned_mss.xy() = accel_ne_mss;
1197 3029099 : }
1198 :
1199 : // Converts lean angles (rad) to NED acceleration in m/s².
1200 9776429 : Vector3f AC_PosControl::lean_angles_rad_to_accel_NED_mss(const Vector3f& att_target_euler_rad) const
1201 : {
1202 : // rotate our roll, pitch angles into lat/lon frame
1203 9776429 : const float sin_roll = sinf(att_target_euler_rad.x);
1204 9776429 : const float cos_roll = cosf(att_target_euler_rad.x);
1205 9776429 : const float sin_pitch = sinf(att_target_euler_rad.y);
1206 9776429 : const float cos_pitch = cosf(att_target_euler_rad.y);
1207 9776429 : const float sin_yaw = sinf(att_target_euler_rad.z);
1208 9776429 : const float cos_yaw = cosf(att_target_euler_rad.z);
1209 :
1210 29329287 : return Vector3f{
1211 9776429 : GRAVITY_MSS * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
1212 9776429 : GRAVITY_MSS * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
1213 : -GRAVITY_MSS
1214 9776429 : };
1215 : }
1216 :
1217 : /// Terrain
1218 :
1219 : // Initializes terrain position, velocity, and acceleration to match the terrain target.
1220 6184169 : void AC_PosControl::init_terrain()
1221 : {
1222 : // set terrain position and target to zero
1223 6184169 : _pos_terrain_target_d_m = 0.0;
1224 6184169 : _pos_terrain_d_m = 0.0;
1225 :
1226 : // set velocity offset to zero
1227 6184169 : _vel_terrain_d_ms = 0.0;
1228 :
1229 : // set acceleration offset to zero
1230 6184169 : _accel_terrain_d_mss = 0.0;
1231 6184169 : }
1232 :
1233 : // Initializes both the terrain altitude and terrain target to the same value
1234 : // (altitude above EKF origin in centimeters, Up-positive).
1235 : // See init_pos_terrain_D_m() for full description.
1236 4809 : void AC_PosControl::init_pos_terrain_U_cm(float pos_terrain_u_cm)
1237 : {
1238 4809 : init_pos_terrain_D_m(-pos_terrain_u_cm * 0.01);
1239 4809 : }
1240 :
1241 : // Initializes both the terrain altitude and terrain target to the same value
1242 : // (relative to EKF origin in meters, Down-positive).
1243 5390 : void AC_PosControl::init_pos_terrain_D_m(float pos_terrain_d_m)
1244 : {
1245 5390 : _pos_desired_ned_m.z -= (pos_terrain_d_m - _pos_terrain_d_m);
1246 5390 : _pos_terrain_target_d_m = pos_terrain_d_m;
1247 5390 : _pos_terrain_d_m = pos_terrain_d_m;
1248 5390 : }
1249 :
1250 :
1251 : /// Offsets
1252 :
1253 : // Initializes NE position/velocity/acceleration offsets to match their respective targets.
1254 3708875 : void AC_PosControl::NE_init_offsets()
1255 : {
1256 : // check for offset target timeout
1257 3708875 : uint32_t now_ms = AP_HAL::millis();
1258 3708875 : if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
1259 3706341 : _pos_offset_target_ned_m.xy().zero();
1260 3706341 : _vel_offset_target_ned_ms.xy().zero();
1261 3706341 : _accel_offset_target_ned_mss.xy().zero();
1262 : }
1263 :
1264 : // set position offset to target
1265 3708875 : _pos_offset_ned_m.xy() = _pos_offset_target_ned_m.xy();
1266 :
1267 : // set velocity offset to target
1268 3708875 : _vel_offset_ned_ms.xy() = _vel_offset_target_ned_ms.xy();
1269 :
1270 : // set acceleration offset to target
1271 3708875 : _accel_offset_ned_mss.xy() = _accel_offset_target_ned_mss.xy();
1272 3708875 : }
1273 :
1274 : // Initializes vertical (U) offsets to match their respective targets.
1275 6184169 : void AC_PosControl::D_init_offsets()
1276 : {
1277 : // check for offset target timeout
1278 6184169 : uint32_t now_ms = AP_HAL::millis();
1279 6184169 : if (now_ms - _posvelaccel_offset_target_d_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
1280 6103797 : _pos_offset_target_ned_m.z = 0.0;
1281 6103797 : _vel_offset_target_ned_ms.z = 0.0;
1282 6103797 : _accel_offset_target_ned_mss.z = 0.0;
1283 : }
1284 :
1285 : // set position offset to target
1286 6184169 : _pos_offset_ned_m.z = _pos_offset_target_ned_m.z;
1287 :
1288 : // set velocity offset to target
1289 6184169 : _vel_offset_ned_ms.z = _vel_offset_target_ned_ms.z;
1290 :
1291 : // set acceleration offset to target
1292 6184169 : _accel_offset_ned_mss.z = _accel_offset_target_ned_mss.z;
1293 6184169 : }
1294 :
1295 : #if AP_SCRIPTING_ENABLED
1296 : // Sets additional position, velocity, and acceleration offsets in meters (NED frame) for scripting.
1297 : // Offsets are added to the controller’s internal target.
1298 : // Used in LUA
1299 1257 : bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_ned_m, const Vector3f &vel_offset_ned_ms, const Vector3f &accel_offset_ned_mss)
1300 : {
1301 1257 : set_posvelaccel_offset_target_NE_m(pos_offset_ned_m.topostype().xy(), vel_offset_ned_ms.xy(), accel_offset_ned_mss.xy());
1302 1257 : set_posvelaccel_offset_target_D_m(pos_offset_ned_m.topostype().z, vel_offset_ned_ms.z, accel_offset_ned_mss.z);
1303 1257 : return true;
1304 : }
1305 :
1306 : // Retrieves current scripted offsets in meters (NED frame).
1307 : // Used in LUA
1308 13 : bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_ned_m, Vector3f &vel_offset_ned_ms, Vector3f &accel_offset_ned_mss)
1309 : {
1310 13 : pos_offset_ned_m = _pos_offset_target_ned_m.tofloat();
1311 13 : vel_offset_ned_ms = _vel_offset_target_ned_ms;
1312 13 : accel_offset_ned_mss = _accel_offset_target_ned_mss;
1313 13 : return true;
1314 : }
1315 :
1316 : // Retrieves current target velocity (NED frame, m/s) including any scripted offset.
1317 : // Used in LUA
1318 0 : bool AC_PosControl::get_vel_target(Vector3f &vel_target_ned_ms)
1319 : {
1320 0 : if (!NE_is_active() || !D_is_active()) {
1321 0 : return false;
1322 : }
1323 :
1324 0 : vel_target_ned_ms = _vel_target_ned_ms;
1325 0 : return true;
1326 : }
1327 :
1328 : // Retrieves current target acceleration (NED frame, m/s²) including any scripted offset.
1329 : // Used in LUA
1330 0 : bool AC_PosControl::get_accel_target(Vector3f &accel_target_ned_mss)
1331 : {
1332 0 : if (!NE_is_active() || !D_is_active()) {
1333 0 : return false;
1334 : }
1335 :
1336 0 : accel_target_ned_mss = _accel_target_ned_mss;
1337 0 : return true;
1338 : }
1339 : #endif
1340 :
1341 : // Sets NE offset targets in meters, m/s, and m/s².
1342 1257 : void AC_PosControl::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)
1343 : {
1344 : // set position offset target
1345 1257 : _pos_offset_target_ned_m.xy() = pos_offset_target_ne_m;
1346 :
1347 : // set velocity offset target
1348 1257 : _vel_offset_target_ned_ms.xy() = vel_offset_target_ne_ms;
1349 :
1350 : // set acceleration offset target
1351 1257 : _accel_offset_target_ned_mss.xy() = accel_offset_target_ne_mss;
1352 :
1353 : // record time of update so we can detect timeouts
1354 1257 : _posvelaccel_offset_target_ne_ms = AP_HAL::millis();
1355 1257 : }
1356 :
1357 : // Sets vertical offset targets (m, m/s, m/s²) relative to EKF origin in meters, Down-positive.
1358 1257 : void AC_PosControl::set_posvelaccel_offset_target_D_m(float pos_offset_target_d_m, float vel_offset_target_d_ms, const float accel_offset_target_d_mss)
1359 : {
1360 : // set position offset target
1361 1257 : _pos_offset_target_ned_m.z = pos_offset_target_d_m;
1362 :
1363 : // set velocity offset target
1364 1257 : _vel_offset_target_ned_ms.z = vel_offset_target_d_ms;
1365 :
1366 : // set acceleration offset target
1367 1257 : _accel_offset_target_ned_mss.z = accel_offset_target_d_mss;
1368 :
1369 : // record time of update so we can detect timeouts
1370 1257 : _posvelaccel_offset_target_d_ms = AP_HAL::millis();
1371 1257 : }
1372 :
1373 : // Returns desired thrust direction as a unit vector in the body frame.
1374 15470758 : Vector3f AC_PosControl::get_thrust_vector() const
1375 : {
1376 15470758 : Vector3f accel_target_ned_mss = get_accel_target_NED_mss();
1377 15470758 : accel_target_ned_mss.z = -GRAVITY_MSS;
1378 15470758 : return accel_target_ned_mss;
1379 : }
1380 :
1381 : // Computes NE stopping point in meters based on current position, velocity, and acceleration.
1382 39014 : void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const
1383 : {
1384 : // Start from estimated NE position with offset removed
1385 : // todo: we should use the current target position and velocity if we are currently running the position controller
1386 39014 : stopping_point_ne_m = _pos_estimate_ned_m.xy();
1387 39014 : stopping_point_ne_m -= _pos_offset_ned_m.xy();
1388 :
1389 39014 : Vector2f vel_estimate_ne_ms = _vel_estimate_ned_ms.xy();
1390 39014 : vel_estimate_ne_ms -= _vel_offset_ned_ms.xy();
1391 :
1392 : // Compute velocity magnitude
1393 39014 : float speed_ne_ms = vel_estimate_ne_ms.length();
1394 :
1395 39014 : if (!is_positive(speed_ne_ms)) {
1396 449 : return;
1397 : }
1398 :
1399 : // Use current P gain and max accel to estimate stopping distance
1400 38565 : float kP = _p_pos_ne_m.kP();
1401 38565 : const float stopping_dist_m = stopping_distance(constrain_float(speed_ne_ms, 0.0, _vel_max_ne_ms), kP, _accel_max_ne_mss);
1402 38565 : if (!is_positive(stopping_dist_m)) {
1403 0 : return;
1404 : }
1405 :
1406 : // Project stopping distance along current velocity direction
1407 : // todo: convert velocity to a unit vector instead.
1408 38565 : const float stopping_time_s = stopping_dist_m / speed_ne_ms;
1409 38565 : stopping_point_ne_m += (vel_estimate_ne_ms * stopping_time_s).topostype();
1410 : }
1411 :
1412 : // Computes vertical stopping point in meters based on current velocity and acceleration.
1413 32936 : void AC_PosControl::get_stopping_point_D_m(postype_t &stopping_point_d_m) const
1414 : {
1415 32936 : float curr_pos_d_m = _pos_estimate_ned_m.z;
1416 32936 : curr_pos_d_m -= _pos_offset_ned_m.z;
1417 :
1418 32936 : float curr_vel_d_ms = _vel_estimate_ned_ms.z;
1419 32936 : curr_vel_d_ms -= _vel_offset_ned_ms.z;
1420 :
1421 : // If controller is unconfigured or disabled, return current position
1422 32936 : if (!is_positive(_p_pos_d_m.kP()) || !is_positive(_accel_max_d_mss)) {
1423 0 : stopping_point_d_m = curr_pos_d_m;
1424 0 : return;
1425 : }
1426 :
1427 : // Estimate stopping point using current velocity, P gain, and max vertical acceleration
1428 32936 : stopping_point_d_m = curr_pos_d_m + constrain_float(stopping_distance(curr_vel_d_ms, _p_pos_d_m.kP(), _accel_max_d_mss), - POSCONTROL_STOPPING_DIST_UP_MAX_M, POSCONTROL_STOPPING_DIST_DOWN_MAX_M);
1429 : }
1430 :
1431 : // Returns bearing from current position to position target in radians.
1432 : // 0 = North, positive = clockwise.
1433 54120 : float AC_PosControl::get_bearing_to_target_rad() const
1434 : {
1435 54120 : return (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).angle();
1436 : }
1437 :
1438 :
1439 : ///
1440 : /// System methods
1441 : ///
1442 :
1443 : // Updates internal NED position and velocity estimates from AHRS.
1444 : // Falls back to vertical-only data if horizontal velocity or position is invalid or vibration forces it.
1445 : // When high_vibes is true, forces use of vertical fallback for velocity.
1446 39097185 : void AC_PosControl::update_estimates(bool high_vibes)
1447 : {
1448 39097185 : Vector3p pos_estimate_ned_m;
1449 39097185 : if (!AP::ahrs().get_relative_position_NED_origin(pos_estimate_ned_m)) {
1450 : float posD;
1451 7851347 : if (AP::ahrs().get_relative_position_D_origin_float(posD)) {
1452 6204910 : pos_estimate_ned_m.z = posD;
1453 : }
1454 : }
1455 39097185 : _pos_estimate_ned_m = pos_estimate_ned_m;
1456 :
1457 39097185 : Vector3f vel_estimate_ned_ms;
1458 39097185 : if (!AP::ahrs().get_velocity_NED(vel_estimate_ned_ms) || high_vibes) {
1459 : float rate_z;
1460 1109990 : if (AP::ahrs().get_vert_pos_rate_D(rate_z)) {
1461 1100361 : vel_estimate_ned_ms.z = rate_z;
1462 : }
1463 : }
1464 39097185 : _vel_estimate_ned_ms = vel_estimate_ned_ms;
1465 39097185 : }
1466 :
1467 : // Calculates vertical throttle using vibration-resistant feedforward estimation.
1468 : // Returns throttle output using manual feedforward gain for vibration compensation mode.
1469 : // Integrator is adjusted using velocity error when PID is being overridden.
1470 11757 : float AC_PosControl::get_throttle_with_vibration_override()
1471 : {
1472 11757 : const float thr_per_accel_d_mss = _motors.get_throttle_hover();
1473 : // Estimate throttle based on desired acceleration (manual feedforward gain).
1474 : // Used when IMU vibrations corrupt raw acceleration measurements.
1475 : // Allow integrator to compensate for velocity error only if not thrust-limited,
1476 : // or if integrator is actively helping counteract velocity error direction.
1477 : // ToDo: clear pid_info P, I and D terms for logging
1478 11757 : if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_d_m.get_i()) && is_negative(_pid_vel_d_m.get_error())) || (is_negative(_pid_accel_d_m.get_i()) && is_positive(_pid_vel_d_m.get_error())))) {
1479 : // Adjust integrator to help reduce velocity error.
1480 : // Note: scale by velocity P-gain and an override-specific I gain.
1481 10346 : _pid_accel_d_m.set_integrator(_pid_accel_d_m.get_i() + _dt_s * thr_per_accel_d_mss * _pid_vel_d_m.get_error() * _pid_vel_d_m.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
1482 : }
1483 : // Final throttle = P term (feedforward) + scaled I term.
1484 11757 : return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_d_mss * _accel_target_ned_mss.z + _pid_accel_d_m.get_i();
1485 : }
1486 :
1487 : // Resets NED position controller state to prevent transients when exiting standby.
1488 : // Zeros I-terms and aligns targets to current position.
1489 0 : void AC_PosControl::NED_standby_reset()
1490 : {
1491 : // Reset vertical acceleration controller integrator to prevent throttle bias on reentry
1492 0 : _pid_accel_d_m.set_integrator(0.0f);
1493 :
1494 : // Reset position controller targets to match current estimate — avoids position jumps
1495 0 : _pos_target_ned_m = _pos_estimate_ned_m;
1496 :
1497 : // Reset horizontal velocity controller integrator and derivative filter
1498 0 : _pid_vel_ne_m.reset_filter();
1499 :
1500 : // Reset EKF XY position reset tracking for NE controller
1501 0 : NE_init_ekf_reset();
1502 0 : }
1503 :
1504 : #if HAL_LOGGING_ENABLED
1505 : // Writes position controller diagnostic logs (PSCN, PSCE, etc).
1506 796093 : void AC_PosControl::write_log()
1507 : {
1508 796093 : if (NE_is_active()) {
1509 : float accel_n_mss, accel_e_mss;
1510 615658 : lean_angles_to_accel_NE_mss(accel_n_mss, accel_e_mss);
1511 :
1512 : // Log North-axis position control (PSCN): desired, target, and actual
1513 615658 : Write_PSCN(_pos_desired_ned_m.x, _pos_target_ned_m.x, _pos_estimate_ned_m.x ,
1514 : _vel_desired_ned_ms.x, _vel_target_ned_ms.x, _vel_estimate_ned_ms.x,
1515 : _accel_desired_ned_mss.x, _accel_target_ned_mss.x, accel_n_mss);
1516 :
1517 : // Log East-axis position control (PSCE): desired, target, and actual
1518 615658 : Write_PSCE(_pos_desired_ned_m.y, _pos_target_ned_m.y, _pos_estimate_ned_m.y,
1519 : _vel_desired_ned_ms.y, _vel_target_ned_ms.y, _vel_estimate_ned_ms.y,
1520 : _accel_desired_ned_mss.y, _accel_target_ned_mss.y, accel_e_mss);
1521 :
1522 : // log offsets if they are being used
1523 615658 : if (!_pos_offset_ned_m.xy().is_zero()) {
1524 : // Log North offset tracking (PSON)
1525 295 : Write_PSON(_pos_offset_target_ned_m.x, _pos_offset_ned_m.x, _vel_offset_target_ned_ms.x, _vel_offset_ned_ms.x, _accel_offset_target_ned_mss.x, _accel_offset_ned_mss.x);
1526 :
1527 : // Log East offset tracking (PSOE)
1528 295 : Write_PSOE(_pos_offset_target_ned_m.y, _pos_offset_ned_m.y, _vel_offset_target_ned_ms.y, _vel_offset_ned_ms.y, _accel_offset_target_ned_mss.y, _accel_offset_ned_mss.y);
1529 : }
1530 : }
1531 :
1532 796093 : if (D_is_active()) {
1533 : // Log Down-axis position control (PSCD)
1534 763268 : Write_PSCD(_pos_desired_ned_m.z, _pos_target_ned_m.z, _pos_estimate_ned_m.z,
1535 : _vel_desired_ned_ms.z, _vel_target_ned_ms.z, _vel_estimate_ned_ms.z,
1536 : _accel_desired_ned_mss.z, _accel_target_ned_mss.z, get_estimated_accel_D_mss());
1537 :
1538 : // log down and terrain offsets if they are being used
1539 763268 : if (!is_zero(_pos_offset_ned_m.z)) {
1540 : // Log Down offset tracking (PSOD)
1541 713 : Write_PSOD(_pos_offset_target_ned_m.z, _pos_offset_ned_m.z, _vel_offset_target_ned_ms.z, _vel_offset_ned_ms.z, _accel_offset_target_ned_mss.z, _accel_offset_ned_mss.z);
1542 : }
1543 763268 : if (!is_zero(_pos_terrain_d_m)) {
1544 : // Log terrain-following offset (PSOT)
1545 17675 : Write_PSOT(_pos_terrain_target_d_m, _pos_terrain_d_m, 0, _vel_terrain_d_ms, 0, _accel_terrain_d_mss);
1546 : }
1547 : }
1548 796093 : }
1549 : #endif // HAL_LOGGING_ENABLED
1550 :
1551 : // Returns lateral distance to closest point on active trajectory in meters.
1552 : // Used to assess horizontal deviation from path.
1553 279331 : float AC_PosControl::crosstrack_error_m() const
1554 : {
1555 279331 : const Vector2f pos_error = (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy()).tofloat();
1556 279331 : if (is_zero(_vel_desired_ned_ms.xy().length_squared())) {
1557 : // No desired velocity → return direct distance to target
1558 156739 : return pos_error.length();
1559 : } else {
1560 : // Project position error onto desired velocity vector
1561 122592 : const Vector2f vel_unit = _vel_desired_ned_ms.xy().normalized();
1562 122592 : const float dot_error = pos_error * vel_unit;
1563 :
1564 : // Use Pythagorean difference to isolate perpendicular (cross-track) component
1565 : // todo: remove MAX of zero when safe_sqrt fixed
1566 122592 : return safe_sqrt(MAX(pos_error.length_squared() - sq(dot_error), 0.0));
1567 : }
1568 : }
1569 :
1570 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1571 : // Returns true if the requested forward pitch is limited by the configured tilt constraint.
1572 21066 : bool AC_PosControl::get_fwd_pitch_is_limited() const
1573 : {
1574 21066 : if (_limit_vector_ned.xy().is_zero()) {
1575 18302 : return false;
1576 : }
1577 2764 : const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad());
1578 2764 : const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad);
1579 : // Check for pitch limiting in the forward direction
1580 2764 : const float accel_fwd_unlimited_mss = _limit_vector_ned.x * _ahrs.cos_yaw() + _limit_vector_ned.y * _ahrs.sin_yaw();
1581 2764 : const float pitch_target_unlimited_deg = accel_mss_to_angle_deg(- MIN(accel_fwd_unlimited_mss, accel_max_mss));
1582 2764 : const float accel_fwd_limited = _accel_target_ned_mss.x * _ahrs.cos_yaw() + _accel_target_ned_mss.y * _ahrs.sin_yaw();
1583 2764 : const float pitch_target_limited_deg = accel_mss_to_angle_deg(- accel_fwd_limited);
1584 :
1585 2764 : return is_negative(pitch_target_unlimited_deg) && pitch_target_unlimited_deg < pitch_target_limited_deg;
1586 : }
1587 : #endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1588 :
1589 : ///
1590 : /// private methods
1591 : ///
1592 :
1593 : /// Terrain
1594 :
1595 : // Updates terrain estimate (_pos_terrain_d_m) toward target using filter time constants.
1596 25567102 : void AC_PosControl::update_terrain()
1597 : {
1598 : // update position, velocity, acceleration offsets for this iteration
1599 25567102 : postype_t pos_terrain_d_m = _pos_terrain_d_m;
1600 25567102 : update_pos_vel_accel(pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss, _dt_s, MIN(_limit_vector_ned.z, 0.0f), _p_pos_d_m.get_error(), _pid_vel_d_m.get_error());
1601 25567102 : _pos_terrain_d_m = pos_terrain_d_m;
1602 :
1603 : // input shape horizontal position, velocity and acceleration offsets
1604 51134204 : shape_pos_vel_accel(_pos_terrain_target_d_m, 0.0, 0.0,
1605 25567102 : _pos_terrain_d_m, _vel_terrain_d_ms, _accel_terrain_d_mss,
1606 25567102 : -get_max_speed_up_ms(), get_max_speed_down_ms(),
1607 25567102 : -D_get_max_accel_mss(), D_get_max_accel_mss(),
1608 : _jerk_max_d_msss, _dt_s, false);
1609 :
1610 : // we do not have to update _pos_terrain_target_d_m because we assume the target velocity and acceleration are zero
1611 : // if we know how fast the terrain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_d_m here
1612 25567102 : }
1613 :
1614 : // Converts horizontal acceleration (m/s²) to roll/pitch lean angles in radians.
1615 19264067 : void AC_PosControl::accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const
1616 : {
1617 : // rotate accelerations into body forward-right frame
1618 19264067 : const float accel_forward_mss = accel_n_mss * _ahrs.cos_yaw() + accel_e_mss * _ahrs.sin_yaw();
1619 19264067 : const float accel_right_mss = -accel_n_mss * _ahrs.sin_yaw() + accel_e_mss * _ahrs.cos_yaw();
1620 :
1621 : // update angle targets that will be passed to stabilize controller
1622 19264067 : pitch_target_rad = accel_mss_to_angle_rad(-accel_forward_mss);
1623 19264067 : float cos_pitch_target = cosf(pitch_target_rad);
1624 19264067 : roll_target_rad = accel_mss_to_angle_rad(accel_right_mss * cos_pitch_target);
1625 19264067 : }
1626 :
1627 : // Converts current target lean angles to NE acceleration in m/s².
1628 616623 : void AC_PosControl::lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const
1629 : {
1630 : // rotate our roll, pitch angles into lat/lon frame
1631 616623 : Vector3f att_target_euler_rad = _attitude_control.get_att_target_euler_rad();
1632 616623 : att_target_euler_rad.z = _ahrs.yaw;
1633 616623 : Vector3f accel_ne_mss = lean_angles_rad_to_accel_NED_mss(att_target_euler_rad);
1634 :
1635 616623 : accel_n_mss = accel_ne_mss.x;
1636 616623 : accel_e_mss = accel_ne_mss.y;
1637 616623 : }
1638 :
1639 : // Computes desired yaw and yaw rate based on the NE acceleration and velocity vectors.
1640 : // Aligns yaw with the direction of travel if speed exceeds 5% of maximum.
1641 19264067 : void AC_PosControl::calculate_yaw_and_rate_yaw()
1642 : {
1643 : // Calculate the turn rate
1644 19264067 : float turn_rate_rads = 0.0f;
1645 19264067 : const float vel_desired_length_ne_ms = _vel_desired_ned_ms.xy().length();
1646 19264067 : if (is_positive(vel_desired_length_ne_ms)) {
1647 : // Project acceleration vector into velocity direction to extract forward acceleration component
1648 11465590 : const float accel_forward_mss = (_accel_desired_ned_mss.x * _vel_desired_ned_ms.x + _accel_desired_ned_mss.y * _vel_desired_ned_ms.y) / vel_desired_length_ne_ms;
1649 : // Subtract forward component to isolate turn acceleration perpendicular to velocity vector
1650 11465590 : const Vector2f accel_turn_ne_mss = _accel_desired_ned_mss.xy() - _vel_desired_ned_ms.xy() * accel_forward_mss / vel_desired_length_ne_ms;
1651 : // Compute turn rate from lateral acceleration and velocity (centripetal formula)
1652 11465590 : const float accel_turn_length_ne_mss = accel_turn_ne_mss.length();
1653 11465590 : turn_rate_rads = accel_turn_length_ne_mss / vel_desired_length_ne_ms;
1654 : // Determine turn direction: positive = clockwise (right)
1655 11465590 : if ((accel_turn_ne_mss.y * _vel_desired_ned_ms.x - accel_turn_ne_mss.x * _vel_desired_ned_ms.y) < 0.0) {
1656 3202530 : turn_rate_rads = -turn_rate_rads;
1657 : }
1658 : }
1659 :
1660 : // If vehicle is moving significantly, align yaw to velocity vector and apply computed turn rate
1661 19264067 : if (vel_desired_length_ne_ms > _vel_max_ne_ms * 0.05f) {
1662 4541950 : _yaw_target_rad = _vel_desired_ned_ms.xy().angle();
1663 4541950 : _yaw_rate_target_rads = turn_rate_rads;
1664 4541950 : return;
1665 : }
1666 :
1667 : // If motion is too slow, retain last yaw target from attitude controller
1668 14722117 : _yaw_target_rad = _attitude_control.get_att_target_euler_rad().z;
1669 14722117 : _yaw_rate_target_rads = 0;
1670 : }
1671 :
1672 : // Computes scaling factor to increase max vertical accel/jerk if vertical speed exceeds configured limits.
1673 17610437 : float AC_PosControl::calculate_overspeed_gain()
1674 : {
1675 : // If desired descent speed exceeds configured max, scale acceleration/jerk proportionally
1676 17610437 : if (_vel_desired_ned_ms.z > _vel_max_down_ms && !is_zero(_vel_max_down_ms)) {
1677 68437 : return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_down_ms;
1678 : }
1679 :
1680 : // If desired climb speed exceeds configured max, scale acceleration/jerk proportionally
1681 17542000 : if (_vel_desired_ned_ms.z < -_vel_max_up_ms && !is_zero(_vel_max_up_ms)) {
1682 38508 : return -POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_ned_ms.z / _vel_max_up_ms;
1683 : }
1684 :
1685 : // Within normal speed limits — use nominal acceleration and jerk
1686 17503492 : return 1.0;
1687 : }
1688 :
1689 : // Initializes tracking of NE EKF position resets.
1690 3708875 : void AC_PosControl::NE_init_ekf_reset()
1691 : {
1692 3708875 : _ahrs_position_NE_reset_count = _ahrs.get_position_NE_reset_count();
1693 3708875 : }
1694 :
1695 : // Handles NE position reset detection and response (e.g., clearing accumulated errors).
1696 19264067 : void AC_PosControl::NE_handle_ekf_reset()
1697 : {
1698 : // Check for EKF-reported NE position reset since last update
1699 19264067 : const uint16_t reset_count = _ahrs.get_position_NE_reset_count();
1700 :
1701 19264067 : if (reset_count != _ahrs_position_NE_reset_count) {
1702 : // This ensures controller output remains continuous after EKF realigns the origin.
1703 :
1704 : // Reconstruct position target relative to the to new EKF estimation to maintain the current position error
1705 36 : Vector2p delta_pos_estimate_ne_m = _p_pos_ne_m.get_error().topostype() - (_pos_target_ned_m.xy() - _pos_estimate_ned_m.xy());
1706 36 : _pos_target_ned_m.xy() += delta_pos_estimate_ne_m;
1707 :
1708 : // Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
1709 36 : Vector2f delta_vel_estimate_ne_ms = _pid_vel_ne_m.get_error() - (_vel_target_ned_ms.xy() - _vel_estimate_ned_ms.xy());
1710 36 : _vel_target_ned_ms.xy() += delta_vel_estimate_ne_ms;
1711 :
1712 36 : switch (_ekf_reset_method) {
1713 33 : case EKFResetMethod::MoveTarget:
1714 : // Reset NE controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
1715 33 : _pos_desired_ned_m.xy() += delta_pos_estimate_ne_m;
1716 33 : _vel_desired_ned_ms.xy() += delta_vel_estimate_ne_ms;
1717 33 : break;
1718 3 : case EKFResetMethod::MoveVehicle:
1719 : // Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
1720 3 : _pos_offset_ned_m.xy() += delta_pos_estimate_ne_m;
1721 3 : _vel_offset_ned_ms.xy() += delta_vel_estimate_ne_ms;
1722 3 : break;
1723 : }
1724 36 : _ahrs_position_NE_reset_count = reset_count;
1725 : }
1726 19264067 : }
1727 :
1728 : // Initializes tracking of vertical (U) EKF resets.
1729 6184169 : void AC_PosControl::D_init_ekf_reset()
1730 : {
1731 6184169 : _ahrs_position_D_reset_count = _ahrs.get_position_D_reset_count();
1732 6184169 : }
1733 :
1734 : // Handles U EKF reset detection and response.
1735 25567102 : void AC_PosControl::D_handle_ekf_reset()
1736 : {
1737 : // Check for EKF-reported Down-axis reset since last update
1738 25567102 : const uint16_t reset_count = _ahrs.get_position_D_reset_count();
1739 :
1740 25567102 : if (reset_count != _ahrs_position_D_reset_count) {
1741 : // This ensures controller output remains continuous after EKF realigns the origin.
1742 : // Reconstruct position target relative to the to new EKF estimation to maintain the current position error
1743 39 : postype_t delta_pos_estimate_d_m = _p_pos_d_m.get_error() - (_pos_target_ned_m.z - _pos_estimate_ned_m.z);
1744 39 : _pos_target_ned_m.z += delta_pos_estimate_d_m;
1745 :
1746 : // Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
1747 39 : float delta_vel_estimate_d_ms = _pid_vel_d_m.get_error() - (_vel_target_ned_ms.z - _vel_estimate_ned_ms.z);
1748 39 : _vel_target_ned_ms.z += delta_vel_estimate_d_ms;
1749 :
1750 39 : switch (_ekf_reset_method) {
1751 36 : case EKFResetMethod::MoveTarget:
1752 : // Reset U controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
1753 36 : _pos_desired_ned_m.z += delta_pos_estimate_d_m;
1754 36 : _vel_desired_ned_ms.z += delta_vel_estimate_d_ms;
1755 36 : break;
1756 3 : case EKFResetMethod::MoveVehicle:
1757 : // Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
1758 3 : _pos_offset_ned_m.z += delta_pos_estimate_d_m;
1759 3 : _vel_offset_ned_ms.z += delta_vel_estimate_d_ms;
1760 3 : break;
1761 : }
1762 39 : _ahrs_position_D_reset_count = reset_count;
1763 : }
1764 25567102 : }
1765 :
1766 : // Performs pre-arm checks for position control parameters and EKF readiness.
1767 : // Returns false if failure_msg is populated.
1768 99877 : bool AC_PosControl::pre_arm_checks(const char *param_prefix,
1769 : char *failure_msg,
1770 : const uint8_t failure_msg_len)
1771 : {
1772 99877 : if (!is_positive(NE_get_pos_p().kP())) {
1773 4 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_NE_POS_P must be > 0", param_prefix);
1774 4 : return false;
1775 : }
1776 99873 : if (!is_positive(D_get_pos_p().kP())) {
1777 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_POS_P must be > 0", param_prefix);
1778 0 : return false;
1779 : }
1780 99873 : if (!is_positive(D_get_vel_pid().kP())) {
1781 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_VEL_P must be > 0", param_prefix);
1782 0 : return false;
1783 : }
1784 99873 : if (!is_positive(D_get_accel_pid().kP())) {
1785 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_P must be > 0", param_prefix);
1786 0 : return false;
1787 : }
1788 99873 : if (!is_positive(D_get_accel_pid().kI())) {
1789 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_D_ACC_I must be > 0", param_prefix);
1790 0 : return false;
1791 : }
1792 :
1793 99873 : return true;
1794 : }
1795 :
1796 : // return true if on a real vehicle or SITL with lock-step scheduling
1797 0 : bool AC_PosControl::has_good_timing(void) const
1798 : {
1799 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1800 0 : auto *sitl = AP::sitl();
1801 0 : if (sitl) {
1802 0 : return sitl->state.is_lock_step_scheduled;
1803 : }
1804 : #endif
1805 : // real boards are assumed to have good timing
1806 0 : return true;
1807 : }
1808 :
1809 : // perform any required parameter conversions
1810 1202 : void AC_PosControl::convert_parameters()
1811 : {
1812 : // find param table key
1813 : uint16_t k_param_psc_key;
1814 1202 : if (!AP_Param::find_key_by_pointer(this, k_param_psc_key)) {
1815 1035 : return;
1816 : }
1817 :
1818 : // return immediately if parameter conversion has already been performed
1819 1202 : if (_pid_accel_d_m.kP().configured()) {
1820 1035 : return;
1821 : }
1822 :
1823 : // PARAMETER_CONVERSION - Added: Nov-2025 for ArduPilot-4.7
1824 : // parameters that are simply moved
1825 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1826 : static const AP_Param::ConversionInfo conversion_info[] = {
1827 : { k_param_psc_key, 258257, AP_PARAM_FLOAT, "Q_P_D_VEL_P" }, // Q_P_VELZ_P moved to Q_P_D_VEL_P
1828 : { k_param_psc_key, 4305, AP_PARAM_FLOAT, "Q_P_D_VEL_I" }, // Q_P_VELZ_I moved to Q_P_D_VEL_I
1829 : { k_param_psc_key, 16593, AP_PARAM_FLOAT, "Q_P_D_VEL_D" }, // Q_P_VELZ_D moved to Q_P_D_VEL_D
1830 : { k_param_psc_key, 12497, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTE" }, // Q_P_VELZ_FLTE moved to Q_P_D_VEL_FLTE
1831 : { k_param_psc_key, 20689, AP_PARAM_FLOAT, "Q_P_D_VEL_FLTD" }, // Q_P_VELZ_FLTD moved to Q_P_D_VEL_FLTD
1832 : { k_param_psc_key, 24785, AP_PARAM_FLOAT, "Q_P_D_VEL_FF" }, // Q_P_VELZ_FF moved to Q_P_D_VEL_FF
1833 : { k_param_psc_key, 16657, AP_PARAM_FLOAT, "Q_P_D_ACC_FF" }, // Q_P_ACCZ_FF moved to Q_P_D_ACC_FF
1834 : { k_param_psc_key, 37137, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTT" }, // Q_P_ACCZ_FLTT moved to Q_P_D_ACC_FLTT
1835 : { k_param_psc_key, 41233, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTE" }, // Q_P_ACCZ_FLTE moved to Q_P_D_ACC_FLTE
1836 : { k_param_psc_key, 45329, AP_PARAM_FLOAT, "Q_P_D_ACC_FLTD" }, // Q_P_ACCZ_FLTD moved to Q_P_D_ACC_FLTD
1837 : { k_param_psc_key, 49425, AP_PARAM_FLOAT, "Q_P_D_ACC_SMAX" }, // Q_P_ACCZ_SMAX moved to Q_P_D_ACC_SMAX
1838 : { k_param_psc_key, 53521, AP_PARAM_FLOAT, "Q_P_D_ACC_PDMX" }, // Q_P_ACCZ_PDMX moved to Q_P_D_ACC_PDMX
1839 : { k_param_psc_key, 57617, AP_PARAM_FLOAT, "Q_P_D_ACC_D_FF" }, // Q_P_ACCZ_D_FF moved to Q_P_D_ACC_D_FF
1840 : { k_param_psc_key, 65809, AP_PARAM_INT8, "Q_P_D_ACC_NEF" }, // Q_P_ACCZ_NEF moved to Q_P_D_ACC_NEF
1841 : { k_param_psc_key, 61713, AP_PARAM_INT8, "Q_P_D_ACC_NTF" }, // Q_P_ACCZ_NTF moved to Q_P_D_ACC_NTF
1842 : { k_param_psc_key, 258449, AP_PARAM_FLOAT, "Q_P_NE_VEL_P" }, // Q_P_VELXY_P moved to Q_P_NE_VEL_P
1843 : { k_param_psc_key, 4497, AP_PARAM_FLOAT, "Q_P_NE_VEL_I" }, // Q_P_VELXY_I moved to Q_P_NE_VEL_I
1844 : { k_param_psc_key, 16785, AP_PARAM_FLOAT, "Q_P_NE_VEL_D" }, // Q_P_VELXY_D moved to Q_P_NE_VEL_D
1845 : { k_param_psc_key, 12689, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTE" },// Q_P_VELXY_FLTE moved to Q_P_NE_VEL_FLTE
1846 : { k_param_psc_key, 20881, AP_PARAM_FLOAT, "Q_P_NE_VEL_FLTD" },// Q_P_VELXY_FLTD moved to Q_P_NE_VEL_FLTD
1847 : { k_param_psc_key, 24977, AP_PARAM_FLOAT, "Q_P_NE_VEL_FF" }, // Q_P_VELXY_FF moved to Q_P_NE_VEL_FF
1848 46 : };
1849 : #else
1850 : static const AP_Param::ConversionInfo conversion_info[] = {
1851 : { k_param_psc_key, 4035, AP_PARAM_FLOAT, "PSC_D_VEL_P" }, // PSC_VELZ_P moved to PSC_D_VEL_P
1852 : { k_param_psc_key, 67, AP_PARAM_FLOAT, "PSC_D_VEL_I" }, // PSC_VELZ_I moved to PSC_D_VEL_I
1853 : { k_param_psc_key, 259, AP_PARAM_FLOAT, "PSC_D_VEL_D" }, // PSC_VELZ_D moved to PSC_D_VEL_D
1854 : { k_param_psc_key, 195, AP_PARAM_FLOAT, "PSC_D_VEL_FLTE" }, // PSC_VELZ_FLTE moved to PSC_D_VEL_FLTE
1855 : { k_param_psc_key, 323, AP_PARAM_FLOAT, "PSC_D_VEL_FLTD" }, // PSC_VELZ_FLTD moved to PSC_D_VEL_FLTD
1856 : { k_param_psc_key, 387, AP_PARAM_FLOAT, "PSC_D_VEL_FF" }, // PSC_VELZ_FF moved to PSC_D_VEL_FF
1857 : { k_param_psc_key, 260, AP_PARAM_FLOAT, "PSC_D_ACC_FF" }, // PSC_ACCZ_FF moved to PSC_D_ACC_FF
1858 : { k_param_psc_key, 580, AP_PARAM_FLOAT, "PSC_D_ACC_FLTT" }, // PSC_ACCZ_FLTT moved to PSC_D_ACC_FLTT
1859 : { k_param_psc_key, 644, AP_PARAM_FLOAT, "PSC_D_ACC_FLTE" }, // PSC_ACCZ_FLTE moved to PSC_D_ACC_FLTE
1860 : { k_param_psc_key, 708, AP_PARAM_FLOAT, "PSC_D_ACC_FLTD" }, // PSC_ACCZ_FLTD moved to PSC_D_ACC_FLTD
1861 : { k_param_psc_key, 772, AP_PARAM_FLOAT, "PSC_D_ACC_SMAX" }, // PSC_ACCZ_SMAX moved to PSC_D_ACC_SMAX
1862 : { k_param_psc_key, 836, AP_PARAM_FLOAT, "PSC_D_ACC_PDMX" }, // PSC_ACCZ_PDMX moved to PSC_D_ACC_PDMX
1863 : { k_param_psc_key, 900, AP_PARAM_FLOAT, "PSC_D_ACC_D_FF" }, // PSC_ACCZ_D_FF moved to PSC_D_ACC_D_FF
1864 : { k_param_psc_key, 1028, AP_PARAM_INT8, "PSC_D_ACC_NEF" }, // PSC_ACCZ_NEF moved to PSC_D_ACC_NEF
1865 : { k_param_psc_key, 964, AP_PARAM_INT8, "PSC_D_ACC_NTF" }, // PSC_ACCZ_NTF moved to PSC_D_ACC_NTF
1866 : { k_param_psc_key, 4038, AP_PARAM_FLOAT, "PSC_NE_VEL_P" }, // PSC_VELXY_P moved to PSC_NE_VEL_P
1867 : { k_param_psc_key, 70, AP_PARAM_FLOAT, "PSC_NE_VEL_I" }, // PSC_VELXY_I moved to PSC_NE_VEL_I
1868 : { k_param_psc_key, 262, AP_PARAM_FLOAT, "PSC_NE_VEL_D" }, // PSC_VELXY_D moved to PSC_NE_VEL_D
1869 : { k_param_psc_key, 198, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTE" },// PSC_VELXY_FLTE moved to PSC_NE_VEL_FLTE
1870 : { k_param_psc_key, 326, AP_PARAM_FLOAT, "PSC_NE_VEL_FLTD" },// PSC_VELXY_FLTD moved to PSC_NE_VEL_FLTD
1871 : { k_param_psc_key, 390, AP_PARAM_FLOAT, "PSC_NE_VEL_FF" }, // PSC_VELXY_FF moved to PSC_NE_VEL_FF
1872 121 : };
1873 : #endif
1874 167 : AP_Param::convert_old_parameters(conversion_info, ARRAY_SIZE(conversion_info));
1875 :
1876 : // parameters moved and scaled by 0.1
1877 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1878 : static const AP_Param::ConversionInfo conversion_info_01[] = {
1879 : { k_param_psc_key, 258321, AP_PARAM_FLOAT, "Q_P_D_ACC_P" }, // Q_P_ACCZ_P moved to Q_P_D_ACC_P
1880 : { k_param_psc_key, 4369, AP_PARAM_FLOAT, "Q_P_D_ACC_I" }, // Q_P_ACCZ_I moved to Q_P_D_ACC_I
1881 : { k_param_psc_key, 8465, AP_PARAM_FLOAT, "Q_P_D_ACC_D" }, // Q_P_ACCZ_D moved to Q_P_D_ACC_D
1882 46 : };
1883 : #else
1884 : static const AP_Param::ConversionInfo conversion_info_01[] = {
1885 : { k_param_psc_key, 4036, AP_PARAM_FLOAT, "PSC_D_ACC_P" }, // PSC_ACCZ_P moved to PSC_D_ACC_P
1886 : { k_param_psc_key, 68, AP_PARAM_FLOAT, "PSC_D_ACC_I" }, // PSC_ACCZ_I moved to PSC_D_ACC_I
1887 : { k_param_psc_key, 132, AP_PARAM_FLOAT, "PSC_D_ACC_D" }, // PSC_ACCZ_D moved to PSC_D_ACC_D
1888 121 : };
1889 : #endif
1890 167 : AP_Param::convert_old_parameters_scaled(conversion_info_01, ARRAY_SIZE(conversion_info_01), 0.1, 0);
1891 :
1892 : // store PSC_D_ACC_P as flag that parameter conversion was completed
1893 167 : _pid_accel_d_m.kP().save(true);
1894 :
1895 : // parameters moved and scaled by 0.01
1896 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1897 : static const AP_Param::ConversionInfo conversion_info_001[] = {
1898 : { k_param_psc_key, 8401, AP_PARAM_FLOAT, "Q_P_D_VEL_IMAX" }, // Q_P_VELZ_IMAX moved to Q_P_D_VEL_IMAX
1899 : { k_param_psc_key, 8593, AP_PARAM_FLOAT, "Q_P_NE_VEL_IMAX" }, // Q_P_VELXY_IMAX moved to Q_P_NE_VEL_IMAX
1900 46 : };
1901 : #else
1902 : static const AP_Param::ConversionInfo conversion_info_001[] = {
1903 : { k_param_psc_key, 131, AP_PARAM_FLOAT, "PSC_D_VEL_IMAX" }, // PSC_VELZ_IMAX moved to PSC_D_VEL_IMAX
1904 : { k_param_psc_key, 134, AP_PARAM_FLOAT, "PSC_NE_VEL_IMAX" }, // PSC_VELXY_IMAX moved to PSC_NE_VEL_IMAX
1905 121 : };
1906 : #endif
1907 167 : AP_Param::convert_old_parameters_scaled(conversion_info_001, ARRAY_SIZE(conversion_info_001), 0.01, 0);
1908 :
1909 : // parameters moved and scaled by 0.001
1910 : // PSC_ACCZ_IMAX replaced by PSC_D_ACC_IMAX
1911 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1912 46 : static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 20753, AP_PARAM_FLOAT, "Q_P_D_ACC_IMAX" };
1913 : #else
1914 121 : static const AP_Param::ConversionInfo psc_d_acc_imax_info = { k_param_psc_key, 324, AP_PARAM_FLOAT, "PSC_D_ACC_IMAX" };
1915 : #endif
1916 167 : AP_Param::convert_old_parameter(&psc_d_acc_imax_info, 0.001f);
1917 : }
|
