Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include "AC_Loiter.h"
3 : #include <AP_Vehicle/AP_Vehicle_Type.h>
4 : #include <AC_Avoidance/AC_Avoid.h>
5 :
6 : extern const AP_HAL::HAL& hal;
7 :
8 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
9 : #define LOITER_SPEED_DEFAULT_MS 30.0 // Default horizontal loiter speed in m/s.
10 : #define LOITER_BRAKE_ACCEL_DEFAULT_MSS 1.25 // Default maximum braking acceleration when sticks are released (m/s²).
11 : #define LOITER_BRAKE_JERK_DEFAULT_MSSS 2.5 // Default maximum jerk applied during braking transitions (m/s³).
12 : #else
13 : #define LOITER_SPEED_DEFAULT_MS 12.5 // Default horizontal loiter speed in m/s.
14 : #define LOITER_BRAKE_ACCEL_DEFAULT_MSS 2.5 // Default maximum braking acceleration when sticks are released (m/s²).
15 : #define LOITER_BRAKE_JERK_DEFAULT_MSSS 5.0 // Default maximum jerk applied during braking transitions (m/s³).
16 : #endif
17 :
18 : // Common defaults between trad heli and copter
19 : #define LOITER_SPEED_MIN_MS 0.2 // Minimum allowed horizontal loiter speed in m/s.
20 : #define LOITER_ACCEL_MAX_DEFAULT_MSS 5.0 // Default maximum horizontal acceleration in loiter mode (m/s²).
21 : #define LOITER_BRAKE_START_DELAY_DEFAULT_S 1.0 // Delay (in seconds) before braking begins after sticks are released.
22 : #define LOITER_VEL_CORRECTION_MAX_MS 2.0 // Maximum speed (in m/s) used for correcting position errors in loiter.
23 : #define LOITER_POS_CORRECTION_MAX_M 2.0 // Maximum horizontal position error allowed before correction (m).
24 : #define LOITER_ACTIVE_TIMEOUT_MS 200 // Loiter is considered active if updated within the past 200 ms.
25 : #define LOITER_DEFAULT_OPTIONS 1 // Enable Coordinated Turn by default.
26 :
27 : const AP_Param::GroupInfo AC_Loiter::var_info[] = {
28 :
29 : // @Param: ANG_MAX
30 : // @DisplayName: Loiter pilot angle max
31 : // @Description{Copter, Sub}: Loiter maximum pilot requested lean angle. Set to zero for 2/3 of PSC_ANGLE_MAX/ATC_ANGLE_MAX. The maximum vehicle lean angle is still limited by PSC_ANGLE_MAX/ATC_ANGLE_MAX
32 : // @Description: Loiter maximum pilot requested lean angle. Set to zero for 2/3 of Q_P_ANGLE_MAX/Q_A_ANGLE_MAX. The maximum vehicle lean angle is still limited by Q_P_ANGLE_MAX/Q_A_ANGLE_MAX
33 : // @Units: deg
34 : // @Range: 0 45
35 : // @Increment: 1
36 : // @User: Advanced
37 : AP_GROUPINFO("ANG_MAX", 1, AC_Loiter, _angle_max_deg, 0.0f),
38 :
39 : // 2 was SPEED
40 : // 3 was ACC_MAX
41 : // 4 was BRK_ACCEL
42 : // 5 was BRK_JERK
43 :
44 : // @Param: BRK_DELAY
45 : // @DisplayName: Loiter brake start delay (in seconds)
46 : // @Description: Loiter brake start delay (in seconds)
47 : // @Units: s
48 : // @Range: 0 2
49 : // @Increment: 0.1
50 : // @User: Advanced
51 : AP_GROUPINFO("BRK_DELAY", 6, AC_Loiter, _brake_delay_s, LOITER_BRAKE_START_DELAY_DEFAULT_S),
52 :
53 : // @Param: OPTIONS
54 : // @DisplayName: Loiter mode options
55 : // @Description: Enables optional Loiter mode behaviors
56 : // @Bitmask: 0: Enable Coordinated turns
57 : // @User: Standard
58 : AP_GROUPINFO("OPTIONS", 7, AC_Loiter, _options, LOITER_DEFAULT_OPTIONS),
59 :
60 : // @Param: SPEED_MS
61 : // @DisplayName: Loiter Horizontal Maximum Speed
62 : // @Description: Defines the maximum speed in m/s which the aircraft will travel horizontally while in loiter mode
63 : // @Units: m/s
64 : // @Range: 0.20 35
65 : // @Increment: 0.05
66 : // @User: Standard
67 : AP_GROUPINFO("SPEED_MS", 8, AC_Loiter, _speed_max_ne_ms, LOITER_SPEED_DEFAULT_MS),
68 :
69 : // @Param: ACC_MAX_M
70 : // @DisplayName: Loiter maximum correction acceleration
71 : // @Description: Loiter maximum correction acceleration in m/s/s. Higher values cause the copter to correct position errors more aggressively.
72 : // @Units: m/s/s
73 : // @Range: 1 9.81
74 : // @Increment: 0.01
75 : // @User: Advanced
76 : AP_GROUPINFO("ACC_MAX_M", 9, AC_Loiter, _accel_max_ne_mss, LOITER_ACCEL_MAX_DEFAULT_MSS),
77 :
78 : // @Param: BRK_ACC_M
79 : // @DisplayName: Loiter braking acceleration
80 : // @Description: Loiter braking acceleration in m/s/s. Higher values stop the copter more quickly when the stick is centered.
81 : // @Units: m/s/s
82 : // @Range: 0.25 2.5
83 : // @Increment: 0.01
84 : // @User: Advanced
85 : AP_GROUPINFO("BRK_ACC_M", 10, AC_Loiter, _brake_accel_max_mss, LOITER_BRAKE_ACCEL_DEFAULT_MSS),
86 :
87 : // @Param: BRK_JRK_M
88 : // @DisplayName: Loiter braking jerk
89 : // @Description: Loiter braking jerk in m/s/s/s. Higher values will remove braking faster if the pilot moves the sticks during a braking maneuver.
90 : // @Units: m/s/s/s
91 : // @Range: 5 50
92 : // @Increment: 0.01
93 : // @User: Advanced
94 : AP_GROUPINFO("BRK_JRK_M", 11, AC_Loiter, _brake_jerk_max_msss, LOITER_BRAKE_JERK_DEFAULT_MSSS),
95 :
96 : AP_GROUPEND
97 : };
98 :
99 : // Default constructor.
100 : // Note that the Vector/Matrix constructors already implicitly zero
101 : // their values.
102 1285 : AC_Loiter::AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
103 1285 : _ahrs(ahrs),
104 1285 : _pos_control(pos_control),
105 1285 : _attitude_control(attitude_control)
106 : {
107 1285 : AP_Param::setup_object_defaults(this, var_info);
108 1285 : }
109 :
110 : // Sets the initial loiter target position in meters from the EKF origin.
111 : // - position_ne_m: horizontal position in the NE frame, in meters.
112 : // - Initializes internal control state including acceleration targets and feed-forward planning.
113 15 : void AC_Loiter::init_target_m(const Vector2p& position_ne_m)
114 : {
115 15 : sanity_check_params();
116 :
117 : // Configure speed/accel limits in meters using internal parameter (_accel_max_ne_mss)
118 15 : _pos_control.NE_set_correction_speed_accel_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_mss);
119 15 : _pos_control.NE_set_pos_error_max_m(LOITER_POS_CORRECTION_MAX_M);
120 :
121 : // Reset controller state for stationary loiter
122 15 : _pos_control.NE_init_controller_stopping_point();
123 :
124 : // Zero out desired and predicted accelerations and angles
125 15 : _predicted_accel_ne_mss.zero();
126 15 : _desired_accel_ne_mss.zero();
127 15 : _predicted_euler_angle_rad.zero();
128 15 : _brake_accel_mss = 0.0f;
129 :
130 : // Set position target for stationary loiter
131 15 : _pos_control.set_pos_desired_NE_m(position_ne_m);
132 15 : }
133 :
134 : // Initializes the loiter controller using the current position and velocity.
135 : // Updates feed-forward velocity, predicted acceleration, and resets control state.
136 1380431 : void AC_Loiter::init_target()
137 : {
138 1380431 : sanity_check_params();
139 :
140 : // Configure correction speed and acceleration limits (in m/s and m/s²)
141 1380431 : _pos_control.NE_set_correction_speed_accel_m(LOITER_VEL_CORRECTION_MAX_MS, _accel_max_ne_mss);
142 1380431 : _pos_control.NE_set_pos_error_max_m(LOITER_POS_CORRECTION_MAX_M);
143 :
144 : // Apply velocity smoothing: softly transitions target acceleration to zero
145 1380431 : _pos_control.NE_relax_velocity_controller();
146 :
147 : // Initialize prediction state using current acceleration and lean angles
148 1380431 : _predicted_accel_ne_mss = _pos_control.get_accel_target_NED_mss().xy();
149 1380431 : _predicted_euler_angle_rad.x = _pos_control.get_roll_rad();
150 1380431 : _predicted_euler_angle_rad.y = _pos_control.get_pitch_rad();
151 1380431 : _predicted_euler_rate.zero();
152 1380431 : _predicted_euler_accel.zero();
153 1380431 : _brake_accel_mss = 0.0f;
154 1380431 : }
155 :
156 : // Reduces loiter responsiveness for smoother descent during landing.
157 : // Internally softens horizontal control gains.
158 1416114 : void AC_Loiter::soften_for_landing()
159 : {
160 1416114 : _pos_control.NE_soften_for_landing();
161 1416114 : }
162 :
163 : // Sets pilot desired acceleration using Euler angles in centidegrees.
164 : // See set_pilot_desired_acceleration_rad() for full details.
165 636619 : void AC_Loiter::set_pilot_desired_acceleration_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd)
166 : {
167 636619 : set_pilot_desired_acceleration_rad(cd_to_rad(euler_roll_angle_cd), cd_to_rad(euler_pitch_angle_cd));
168 636619 : }
169 :
170 : // Sets pilot desired acceleration using Euler angles in radians.
171 : // - Internally computes a smoothed acceleration vector based on predictive rate shaping.
172 : // - Inputs: `euler_roll_angle_rad`, `euler_pitch_angle_rad` in radians.
173 : // - Applies internal shaping using the current attitude controller dt.
174 4579903 : void AC_Loiter::set_pilot_desired_acceleration_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad)
175 : {
176 4579903 : const float dt_s = _attitude_control.get_dt_s();
177 :
178 : // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically
179 4579903 : const Vector3f desired_euler_rad {euler_roll_angle_rad, euler_pitch_angle_rad, _ahrs.yaw};
180 4579903 : _desired_accel_ne_mss = _pos_control.lean_angles_rad_to_accel_NED_mss(desired_euler_rad).xy();
181 :
182 : // Reset brake timer if there is pilot input
183 4579903 : if (!_desired_accel_ne_mss.is_zero()) {
184 506011 : _brake_timer_ms = AP_HAL::millis();
185 : }
186 :
187 : // Compute attitude error between desired and predicted lean angles
188 4579903 : Vector2f angle_error_euler_rad(wrap_PI(euler_roll_angle_rad - _predicted_euler_angle_rad.x), wrap_PI(euler_pitch_angle_rad - _predicted_euler_angle_rad.y));
189 :
190 : // Predict roll/pitch rate required to achieve target attitude
191 4579903 : _attitude_control.command_model_rate_predictor(angle_error_euler_rad, _predicted_euler_rate, _predicted_euler_accel, dt_s);
192 :
193 : // Update internal attitude estimate for next iteration
194 4579903 : _predicted_euler_angle_rad += _predicted_euler_rate * dt_s;
195 :
196 : // Convert predicted angles into an acceleration vector for braking/shaping
197 4579903 : const Vector3f predicted_euler_rad {_predicted_euler_angle_rad.x, _predicted_euler_angle_rad.y, _ahrs.yaw};
198 4579903 : const Vector3f predicted_accel_ned_mss = _pos_control.lean_angles_rad_to_accel_NED_mss(predicted_euler_rad);
199 :
200 4579903 : _predicted_accel_ne_mss = predicted_accel_ned_mss.xy();
201 :
202 4579903 : if (loiter_option_is_set(LoiterOption::COORDINATED_TURN_ENABLED)) {
203 4493130 : Vector3f target_ang_vel_rads = _attitude_control.get_attitude_target_ang_vel();
204 4493130 : Vector3f desired_velocity_ms = _pos_control.get_vel_desired_NED_ms();
205 4493130 : Vector2f turn_accel_ne_mss = Vector2f(-desired_velocity_ms.y * target_ang_vel_rads.z, desired_velocity_ms.x * target_ang_vel_rads.z);
206 4493130 : _desired_accel_ne_mss += turn_accel_ne_mss;
207 4493130 : _predicted_accel_ne_mss += turn_accel_ne_mss;
208 : }
209 4579903 : }
210 :
211 : // Calculates the expected stopping point based on current velocity and position in the NE frame.
212 : // Result is returned in meters.
213 : // Uses the position controller’s deceleration model.
214 0 : void AC_Loiter::get_stopping_point_NE_m(Vector2f& stopping_point_ne_m) const
215 : {
216 0 : Vector2p stop_ne_m;
217 : // Query stopping point from position controller in postype (float or double)
218 0 : _pos_control.get_stopping_point_NE_m(stop_ne_m);
219 :
220 : // Convert from postype to float (Vector2f)
221 0 : stopping_point_ne_m = stop_ne_m.tofloat();
222 0 : }
223 :
224 : // Returns the maximum pilot-commanded lean angle in centidegrees.
225 : // See get_angle_max_rad() for full details.
226 636619 : float AC_Loiter::get_angle_max_cd() const
227 : {
228 : // Convert radians to centidegrees
229 636619 : return rad_to_cd(get_angle_max_rad());
230 : }
231 :
232 : // Returns the maximum pilot-commanded lean angle in radians.
233 : // - If `_angle_max_deg` is zero, this returns 2/3 of the limiting PSC angle.
234 : // - Otherwise, returns the minimum of `_angle_max_deg` and PSC’s configured angle limit.
235 7196011 : float AC_Loiter::get_angle_max_rad() const
236 : {
237 7196011 : if (!is_positive(_angle_max_deg)) {
238 : // Use 2/3 of the smallest system-wide max lean angle
239 5843138 : return MIN(_attitude_control.lean_angle_max_rad(), _pos_control.get_lean_angle_max_rad()) * (2.0f / 3.0f);
240 : }
241 : // Use configured parameter (in degrees), constrained to PSC limit
242 1352873 : return MIN(radians(_angle_max_deg), _pos_control.get_lean_angle_max_rad());
243 : }
244 :
245 : // Runs the loiter control loop, computing desired acceleration and updating position control.
246 : // If `avoidance_on` is true, velocity is adjusted using avoidance logic before being applied.
247 3029099 : void AC_Loiter::update(bool avoidance_on)
248 : {
249 : // Calculate desired velocity using pilot inputs, braking, and drag
250 3029099 : calc_desired_velocity(avoidance_on);
251 :
252 : // Run position controller to compute desired attitude and thrust
253 3029099 : _pos_control.NE_update_controller();
254 3029099 : }
255 :
256 : // Sets the maximum allowed horizontal loiter speed in m/s.
257 672 : void AC_Loiter::set_speed_max_NE_ms(float speed_max_ne_ms)
258 : {
259 672 : _speed_max_ne_ms.set(MAX(speed_max_ne_ms, LOITER_SPEED_MIN_MS));
260 672 : }
261 :
262 : // perform any required parameter conversions
263 1285 : void AC_Loiter::convert_parameters()
264 : {
265 : // PARAMETER_CONVERSION - Added: Jan-2026 for ArduPilot-4.7
266 :
267 : // return immediately if no conversion is needed
268 1285 : if (_speed_max_ne_ms.configured() || _accel_max_ne_mss.configured() || _brake_accel_max_mss.configured() || _brake_jerk_max_msss.configured()) {
269 11 : return;
270 : }
271 :
272 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
273 : // convert QuadPlane parameters
274 : static const AP_Param::ConversionInfo conversion_info[] = {
275 : { 205, 8379, AP_PARAM_FLOAT, "Q_LOIT_SPEED_MS" }, // Q_LOIT_SPEED moved to Q_LOIT_SPEED_MS
276 : { 205, 12475, AP_PARAM_FLOAT, "Q_LOIT_ACC_MAX_M" }, // Q_LOIT_ACC_MAX moved to Q_LOIT_ACC_MAX_M
277 : { 205, 16571, AP_PARAM_FLOAT, "Q_LOIT_BRK_ACC_M" }, // Q_LOIT_BRK_ACCEL moved to Q_LOIT_BRK_ACC_M
278 : { 205, 20667, AP_PARAM_FLOAT, "Q_LOIT_BRK_JRK_M" }, // Q_LOIT_BRK_JERK moved to Q_LOIT_BRK_JRK_M
279 : };
280 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
281 : // convert Sub parameters
282 : static const AP_Param::ConversionInfo conversion_info[] = {
283 : { 63, 2, AP_PARAM_FLOAT, "LOIT_SPEED_MS" }, // LOIT_SPEED moved to LOIT_SPEED_MS
284 : { 63, 3, AP_PARAM_FLOAT, "LOIT_ACC_MAX_M" }, // LOIT_ACC_MAX moved to LOIT_ACC_MAX_M
285 : { 63, 4, AP_PARAM_FLOAT, "LOIT_BRK_ACC_M" }, // LOIT_BRK_ACCEL moved to LOIT_BRK_ACC_M
286 : { 63, 5, AP_PARAM_FLOAT, "LOIT_BRK_JRK_M" }, // LOIT_BRK_JERK moved to LOIT_BRK_JRK_M
287 : };
288 : #else
289 : // convert Copter parameters
290 : static const AP_Param::ConversionInfo conversion_info[] = {
291 : { 105, 2, AP_PARAM_FLOAT, "LOIT_SPEED_MS" }, // LOIT_SPEED moved to LOIT_SPEED_MS
292 : { 105, 3, AP_PARAM_FLOAT, "LOIT_ACC_MAX_M" }, // LOIT_ACC_MAX moved to LOIT_ACC_MAX_M
293 : { 105, 4, AP_PARAM_FLOAT, "LOIT_BRK_ACC_M" }, // LOIT_BRK_ACCEL moved to LOIT_BRK_ACC_M
294 : { 105, 5, AP_PARAM_FLOAT, "LOIT_BRK_JRK_M" }, // LOIT_BRK_JERK moved to LOIT_BRK_JRK_M
295 : };
296 : #endif
297 :
298 1274 : AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
299 : }
300 :
301 : // Ensures internal parameters are within valid safety limits.
302 : // Applies min/max constraints on speed and acceleration settings.
303 1380446 : void AC_Loiter::sanity_check_params()
304 : {
305 : // Enforce minimum loiter speed
306 1380446 : _speed_max_ne_ms.set(MAX(_speed_max_ne_ms, LOITER_SPEED_MIN_MS));
307 :
308 : // Clamp horizontal accel to lean-angle-limited max
309 1380446 : _accel_max_ne_mss.set(MIN(_accel_max_ne_mss, GRAVITY_MSS * tanf(_attitude_control.lean_angle_max_rad())));
310 1380446 : }
311 :
312 4579903 : bool AC_Loiter::loiter_option_is_set(LoiterOption option) const {
313 4579903 : return (_options & int8_t(option)) != 0;
314 : }
315 :
316 : // Updates feed-forward velocity using pilot-requested acceleration and braking logic.
317 : // - Applies drag and braking forces when sticks are released.
318 : // - Velocity is adjusted for fence/avoidance if enabled.
319 : // - Resulting velocity and acceleration are sent to the position controller.
320 3029099 : void AC_Loiter::calc_desired_velocity(bool avoidance_on)
321 : {
322 : // the estimator might impose limits on maximum velocity (e.g. due
323 : // to the sensors being used to supply its estimated velocity):
324 3029099 : const float ekfGndSpdLimit_ms = AP::ahrs().get_control_ground_speed_limit_ms();
325 :
326 3029099 : const float dt_s = _pos_control.get_dt_s();
327 :
328 : // Apply speed limit from LOITER_SPEED and EKF constraint
329 3029099 : float gnd_speed_limit_ms = MIN(_speed_max_ne_ms, ekfGndSpdLimit_ms);
330 3029099 : gnd_speed_limit_ms = MAX(gnd_speed_limit_ms, LOITER_SPEED_MIN_MS);
331 :
332 : // Determine acceleration limit based on maximum allowed lean angle
333 3029099 : float pilot_acceleration_max_mss = (gnd_speed_limit_ms / _speed_max_ne_ms) * angle_rad_to_accel_mss(get_angle_max_rad());
334 :
335 : // Check for invalid dt
336 3029099 : if (is_negative(dt_s)) {
337 0 : return;
338 : }
339 :
340 : // Integrate predicted acceleration
341 3029099 : Vector2f desired_vel_ne_ms = _pos_control.get_vel_desired_NED_ms().xy();
342 :
343 : // update the desired velocity using our predicted acceleration
344 3029099 : desired_vel_ne_ms += _predicted_accel_ne_mss * dt_s;
345 :
346 3029099 : Vector2f loiter_accel_brake_mss;
347 3029099 : float desired_speed_ms = desired_vel_ne_ms.length();
348 3029099 : if (!is_zero(desired_speed_ms)) {
349 2956906 : Vector2f desired_vel_norm = desired_vel_ne_ms / desired_speed_ms;
350 :
351 : // Apply drag: deceleration proportional to current velocity
352 2956906 : float drag_decel_mss = pilot_acceleration_max_mss * desired_speed_ms / gnd_speed_limit_ms;
353 :
354 : // Determine braking acceleration based on stick release and delay timer, stick must have been released for at least a full loop
355 2956906 : float loiter_brake_accel_mss = 0.0f;
356 2956906 : if ((AP_HAL::millis() - _brake_timer_ms) > MAX(_brake_delay_s, dt_s) * 1000.0) {
357 2432684 : const float brake_gain = _pos_control.NE_get_vel_pid().kP() * 0.5f;
358 2432684 : loiter_brake_accel_mss = constrain_float(sqrt_controller(desired_speed_ms, brake_gain, _brake_jerk_max_msss, dt_s), 0.0f, _brake_accel_max_mss);
359 : }
360 :
361 : // Integrate jerk-limited brake acceleration
362 2956906 : _brake_accel_mss += constrain_float(loiter_brake_accel_mss - _brake_accel_mss, -_brake_jerk_max_msss * dt_s, _brake_jerk_max_msss * dt_s);
363 2956906 : loiter_accel_brake_mss = desired_vel_norm * _brake_accel_mss;
364 :
365 : // Update desired speed based on braking and drag
366 2956906 : desired_speed_ms = MAX(desired_speed_ms - (drag_decel_mss + _brake_accel_mss) * dt_s, 0.0f);
367 2956906 : desired_vel_ne_ms = desired_vel_norm * desired_speed_ms;
368 : }
369 :
370 : // Apply braking acceleration to overall feed-forward acceleration
371 3029099 : _desired_accel_ne_mss -= loiter_accel_brake_mss;
372 :
373 : // Apply final velocity magnitude constraint
374 3029099 : float desired_vel_ms = desired_vel_ne_ms.length();
375 3029099 : if (desired_vel_ms > gnd_speed_limit_ms) {
376 16634 : desired_vel_ne_ms = desired_vel_ne_ms * gnd_speed_limit_ms / desired_vel_ms;
377 : }
378 :
379 : #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
380 2392480 : if (avoidance_on) {
381 : // Apply fence/obstacle avoidance adjustments (velocity only)
382 : // TODO: We need to also limit the _desired_accel_ne_mss
383 2303168 : AC_Avoid *_avoid = AP::ac_avoid();
384 2303168 : if (_avoid != nullptr) {
385 2296480 : Vector3f avoidance_vel_ned_cms{desired_vel_ne_ms.x * 100.0, desired_vel_ne_ms.y * 100.0, 0.0f};
386 2296480 : _avoid->adjust_velocity(avoidance_vel_ned_cms, _pos_control.NE_get_pos_p().kP(), _accel_max_ne_mss * 100.0, _pos_control.D_get_pos_p().kP(), _pos_control.D_get_max_accel_mss() * 100.0, dt_s);
387 2296480 : desired_vel_ne_ms = avoidance_vel_ned_cms.xy() * 0.01;
388 : }
389 : }
390 : #endif // !APM_BUILD_ArduPlane
391 :
392 : // Retrieve current desired position
393 3029099 : Vector2p desired_pos_ned_m = _pos_control.get_pos_desired_NED_m().xy();
394 :
395 : // Integrate velocity to update desired position
396 3029099 : desired_pos_ned_m += (desired_vel_ne_ms * dt_s).topostype();
397 :
398 : // Send updated position, velocity, and acceleration to the position controller
399 3029099 : _pos_control.set_pos_vel_accel_NE_m(desired_pos_ned_m, desired_vel_ne_ms, _desired_accel_ne_mss);
400 : }
|
