Line data Source code
1 : #include "AC_AttitudeControl.h"
2 : #include <AP_HAL/AP_HAL.h>
3 : #include <AP_Vehicle/AP_Vehicle.h>
4 : #include <AP_Scheduler/AP_Scheduler.h>
5 : #include <AP_Vehicle/AP_Vehicle_Type.h>
6 :
7 : extern const AP_HAL::HAL& hal;
8 :
9 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
10 : // default gains for Plane
11 : # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium
12 : #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control
13 : #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 0
14 : #else
15 : // default gains for Copter and Sub
16 : # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.10f // Crisp
17 : #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control
18 : #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 1
19 : #endif
20 :
21 : // lean angle max (in degrees) for all vehicles
22 : #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MAX 80.0 // lean angle max in degrees
23 :
24 : // default angle max for all vehicles
25 : #ifndef AC_ATTITUDE_CONTROL_ANGLE_MAX_DEFAULT
26 : # define AC_ATTITUDE_CONTROL_ANGLE_MAX_DEFAULT 30.0f // default max lean angle in degrees
27 : #endif
28 :
29 : AC_AttitudeControl *AC_AttitudeControl::_singleton;
30 :
31 : // table of user settable parameters
32 : const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
33 :
34 : // 0, 1 were RATE_RP_MAX, RATE_Y_MAX
35 :
36 : // 2 was SLEW_YAW (in cdeg/s) - moved to RATE_WPY_MAX in deg/s
37 :
38 : // 3 was for ACCEL_RP_MAX
39 :
40 : // 4 was ACCEL_Y_MAX (in cdeg/s/s) - moved to ACC_Y_MAX in deg/s/s
41 :
42 : // @Param: RATE_FF_ENAB
43 : // @DisplayName: Rate Feedforward Enable
44 : // @Description: Controls whether body-frame rate feedforward is enabled or disabled
45 : // @Values: 0:Disabled, 1:Enabled
46 : // @User: Advanced
47 : AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT),
48 :
49 : // 6 was ACCEL_R_MAX (in cdeg/s/s) - moved to ACC_R_MAX in deg/s/s
50 :
51 : // 7 was ACCEL_P_MAX (in cdeg/s/s) - moved to ACC_P_MAX in deg/s/s
52 :
53 : // IDs 8,9,10,11 RESERVED (in use on Solo)
54 :
55 : // @Param: ANGLE_BOOST
56 : // @DisplayName: Angle Boost
57 : // @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude
58 : // @Values: 0:Disabled, 1:Enabled
59 : // @User: Advanced
60 : AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1),
61 :
62 : // @Param: ANG_RLL_P
63 : // @DisplayName: Roll axis angle controller P gain
64 : // @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
65 : // @Range: 3.000 12.000
66 : // @Range{Sub}: 0.0 12.000
67 : // @Increment: 0.01
68 : // @User: Standard
69 : AP_SUBGROUPINFO(_p_angle_roll, "ANG_RLL_", 13, AC_AttitudeControl, AC_P),
70 :
71 : // @Param: ANG_PIT_P
72 : // @DisplayName: Pitch axis angle controller P gain
73 : // @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
74 : // @Range: 3.000 12.000
75 : // @Range{Sub}: 0.0 12.000
76 : // @Increment: 0.01
77 : // @User: Standard
78 : AP_SUBGROUPINFO(_p_angle_pitch, "ANG_PIT_", 14, AC_AttitudeControl, AC_P),
79 :
80 : // @Param: ANG_YAW_P
81 : // @DisplayName: Yaw axis angle controller P gain
82 : // @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
83 : // @Range: 3.000 12.000
84 : // @Range{Sub}: 0.0 6.000
85 : // @Increment: 0.01
86 : // @User: Standard
87 : AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
88 :
89 : // @Param: ANG_LIM_TC
90 : // @DisplayName: Angle Limit (to maintain altitude) Time Constant
91 : // @Description: Angle Limit (to maintain altitude) Time Constant
92 : // @Range: 0.5 10.0
93 : // @User: Advanced
94 : AP_GROUPINFO("ANG_LIM_TC", 16, AC_AttitudeControl, _angle_limit_tc, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT),
95 :
96 : // @Param: RATE_R_MAX
97 : // @DisplayName: Angular Velocity Max for Roll
98 : // @Description: Maximum angular velocity in roll axis
99 : // @Units: deg/s
100 : // @Range: 0 1080
101 : // @Increment: 1
102 : // @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
103 : // @User: Advanced
104 : AP_GROUPINFO("RATE_R_MAX", 17, AC_AttitudeControl, _ang_vel_roll_max_degs, 0.0f),
105 :
106 : // @Param: RATE_P_MAX
107 : // @DisplayName: Angular Velocity Max for Pitch
108 : // @Description: Maximum angular velocity in pitch axis
109 : // @Units: deg/s
110 : // @Range: 0 1080
111 : // @Increment: 1
112 : // @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
113 : // @User: Advanced
114 : AP_GROUPINFO("RATE_P_MAX", 18, AC_AttitudeControl, _ang_vel_pitch_max_degs, 0.0f),
115 :
116 : // @Param: RATE_Y_MAX
117 : // @DisplayName: Angular Velocity Max for Yaw
118 : // @Description: Maximum angular velocity in yaw axis
119 : // @Units: deg/s
120 : // @Range: 0 1080
121 : // @Increment: 1
122 : // @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
123 : // @User: Advanced
124 : AP_GROUPINFO("RATE_Y_MAX", 19, AC_AttitudeControl, _ang_vel_yaw_max_degs, 0.0f),
125 :
126 : // @Param: INPUT_TC
127 : // @DisplayName: Attitude control input time constant
128 : // @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response
129 : // @Units: s
130 : // @Range: 0 1
131 : // @Increment: 0.01
132 : // @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
133 : // @User: Standard
134 : AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),
135 :
136 : // @Param: LAND_R_MULT
137 : // @DisplayName: Landed roll gain multiplier
138 : // @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis.
139 : // @Range: 0.25 1.0
140 : // @User: Advanced
141 : AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0),
142 :
143 : // @Param: LAND_P_MULT
144 : // @DisplayName: Landed pitch gain multiplier
145 : // @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis.
146 : // @Range: 0.25 1.0
147 : // @User: Advanced
148 : AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0),
149 :
150 : // @Param: LAND_Y_MULT
151 : // @DisplayName: Landed yaw gain multiplier
152 : // @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis.
153 : // @Range: 0.25 1.0
154 : // @User: Advanced
155 : AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0),
156 :
157 : // @Param: ANGLE_MAX
158 : // @DisplayName: Angle Max
159 : // @Description: Maximum lean angle in all flight modes
160 : // @Units: deg
161 : // @Increment: 0.1
162 : // @Range: 10.0 80.0
163 : // @User: Standard
164 : AP_GROUPINFO("ANGLE_MAX", 24, AC_AttitudeControl, _angle_max_deg, AC_ATTITUDE_CONTROL_ANGLE_MAX_DEFAULT),
165 :
166 : // @Param: RATE_WPY_MAX
167 : // @DisplayName: Yaw target slew rate
168 : // @Description: Maximum rate the yaw target can be updated in Auto, Guided, Circle, Follow, RTL, SmartRTL, Throw and ZigZag flight modes
169 : // @Units: deg/s
170 : // @Range: 5 180
171 : // @Increment: 1
172 : // @User: Advanced
173 : AP_GROUPINFO("RATE_WPY_MAX", 25, AC_AttitudeControl, _rate_wp_yaw_max_degs, AC_ATTITUDE_CONTROL_RATE_WPY_MAX_DEFAULT),
174 :
175 : // @Param: ACC_Y_MAX
176 : // @DisplayName: Acceleration Max for Yaw
177 : // @Description: Maximum acceleration in yaw axis
178 : // @Units: deg/s/s
179 : // @Range: 0 720
180 : // @Values: 0:Disabled, 90:VerySlow, 180:Slow, 360:Medium, 540:Fast
181 : // @Increment: 10
182 : // @User: Advanced
183 : AP_GROUPINFO("ACC_Y_MAX", 26, AC_AttitudeControl, _accel_yaw_max_degss, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_DEGSS),
184 :
185 : // @Param: ACC_R_MAX
186 : // @DisplayName: Acceleration Max for Roll
187 : // @Description: Maximum acceleration in roll axis
188 : // @Units: deg/s/s
189 : // @Range: 0 1800
190 : // @Increment: 10
191 : // @Values: 0:Disabled, 300:VerySlow, 720:Slow, 1080:Medium, 1620:Fast
192 : // @User: Advanced
193 : AP_GROUPINFO("ACC_R_MAX", 27, AC_AttitudeControl, _accel_roll_max_degss, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_DEGSS),
194 :
195 : // @Param: ACC_P_MAX
196 : // @DisplayName: Acceleration Max for Pitch
197 : // @Description: Maximum acceleration in pitch axis
198 : // @Units: deg/s/s
199 : // @Range: 0 1800
200 : // @Increment: 10
201 : // @Values: 0:Disabled, 300:VerySlow, 720:Slow, 1080:Medium, 1620:Fast
202 : // @User: Advanced
203 : AP_GROUPINFO("ACC_P_MAX", 28, AC_AttitudeControl, _accel_pitch_max_degss, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_DEGSS),
204 :
205 : AP_GROUPEND
206 : };
207 :
208 : constexpr Vector3f AC_AttitudeControl::VECTORF_111;
209 :
210 : // get the slew yaw rate limit in rad/s
211 14797186 : float AC_AttitudeControl::get_slew_yaw_max_rads() const
212 : {
213 14797186 : if (!is_positive(_ang_vel_yaw_max_degs)) {
214 13668447 : return radians(_rate_wp_yaw_max_degs);
215 : }
216 1128739 : return MIN(radians(_ang_vel_yaw_max_degs), radians(_rate_wp_yaw_max_degs));
217 : }
218 :
219 : // get the latest gyro for the purposes of attitude control
220 : // Counter-inuitively the lowest latency for rate control is achieved by running rate control
221 : // *before* attitude control. This is because you want rate control to run as close as possible
222 : // to the time that a gyro sample was read to minimise jitter and control errors. Running rate
223 : // control after attitude control might makes sense logically, but the overhead of attitude
224 : // control calculations (and other updates) compromises the actual rate control.
225 : //
226 : // In the case of running rate control in a separate thread, the ordering between rate and attitude
227 : // updates is less important, except that gyro sample used should be the latest
228 : //
229 : // Currently quadplane runs rate control after attitude control, necessitating the following code
230 : // to minimise latency.
231 : // However this code can be removed once quadplane updates it's structure to run the rate loops before
232 : // the Attitude controller.
233 35665694 : const Vector3f AC_AttitudeControl::get_latest_gyro() const
234 : {
235 : #if AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL
236 : // rate updates happen before attitude updates so the last gyro value is the last rate gyro value
237 : // this also allows a separate rate thread to be the source of gyro data
238 31619119 : return _rate_gyro_rads;
239 : #else
240 : // rate updates happen after attitude updates so the AHRS must be consulted for the last gyro value
241 4046575 : return _ahrs.get_gyro_latest();
242 : #endif
243 : }
244 :
245 : // Ensure attitude controller have zero errors to relax rate controller output
246 2709520 : void AC_AttitudeControl::relax_attitude_controllers()
247 : {
248 : // take a copy of the last gyro used by the rate controller before using it
249 2709520 : Vector3f gyro = get_latest_gyro();
250 : // Initialize the attitude variables to the current attitude
251 2709520 : _ahrs.get_quat_body_to_ned(_attitude_target);
252 2709520 : _attitude_target.to_euler(_euler_angle_target_rad);
253 2709520 : _attitude_ang_error.initialise();
254 :
255 : // Initialize the angular rate variables to the current rate
256 2709520 : _ang_vel_target_rads = gyro;
257 2709520 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
258 :
259 : // Initialize remaining variables
260 2709520 : _thrust_error_angle_rad = 0.0f;
261 :
262 : // Reset the PID filters
263 2709520 : get_rate_roll_pid().reset_filter();
264 2709520 : get_rate_pitch_pid().reset_filter();
265 2709520 : get_rate_yaw_pid().reset_filter();
266 :
267 : // Reset the I terms
268 2709520 : reset_rate_controller_I_terms();
269 : // finally update the attitude target
270 2709520 : _ang_vel_body_rads = gyro;
271 2709520 : }
272 :
273 11544323 : void AC_AttitudeControl::reset_rate_controller_I_terms()
274 : {
275 11544323 : get_rate_roll_pid().reset_I();
276 11544323 : get_rate_pitch_pid().reset_I();
277 11544323 : get_rate_yaw_pid().reset_I();
278 11544323 : }
279 :
280 : // reset rate controller I terms smoothly to zero in 0.5 seconds
281 2502156 : void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
282 : {
283 2502156 : get_rate_roll_pid().relax_integrator(0.0, _dt_s, AC_ATTITUDE_RATE_RELAX_TC);
284 2502156 : get_rate_pitch_pid().relax_integrator(0.0, _dt_s, AC_ATTITUDE_RATE_RELAX_TC);
285 2502156 : get_rate_yaw_pid().relax_integrator(0.0, _dt_s, AC_ATTITUDE_RATE_RELAX_TC);
286 2502156 : }
287 :
288 : // reset the rate controller target loop updates
289 33074393 : void AC_AttitudeControl::rate_controller_target_reset()
290 : {
291 33074393 : _sysid_ang_vel_body_rads.zero();
292 33074393 : _actuator_sysid.zero();
293 33074393 : _pd_scale = VECTORF_111;
294 33074393 : _i_scale = VECTORF_111;
295 33074393 : _angle_P_scale = VECTORF_111;
296 33074393 : }
297 :
298 : // Reduce attitude control gains while landed to stop ground resonance
299 30412448 : void AC_AttitudeControl::landed_gain_reduction(bool landed)
300 : {
301 30412448 : if (is_positive(_input_tc)) {
302 : // use 2.0 x tc to match the response time to 86% commanded
303 30412448 : const float spool_step = _dt_s / (2.0 * _input_tc);
304 30412448 : if (landed) {
305 12010588 : _landed_gain_ratio = MIN(1.0, _landed_gain_ratio + spool_step);
306 : } else {
307 18401860 : _landed_gain_ratio = MAX(0.0, _landed_gain_ratio - spool_step);
308 : }
309 : } else {
310 0 : _landed_gain_ratio = landed ? 1.0 : 0.0;
311 : }
312 30412448 : Vector3f scale_mult = VECTORF_111 * (1.0 - _landed_gain_ratio) + Vector3f{_land_roll_mult, _land_pitch_mult, _land_yaw_mult} * _landed_gain_ratio;
313 30412448 : set_PD_scale_mult(scale_mult);
314 30412448 : set_angle_P_scale_mult(scale_mult);
315 30412448 : }
316 :
317 : // The attitude controller works around the concept of the desired attitude, target attitude
318 : // and measured attitude. The desired attitude is the attitude input into the attitude controller
319 : // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
320 : // to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed
321 : // directly into the rate controllers. The angular error between the measured attitude and the target attitude is
322 : // fed into the angle controller and the output of the angle controller summed at the input of the rate controllers.
323 : // By feeding the target angular velocity directly into the rate controllers the measured and target attitudes
324 : // remain very close together.
325 : //
326 : // All input functions below follow the same procedure
327 : // 1. define the desired attitude or attitude change based on the input variables
328 : // 2. update the target attitude based on the angular velocity target and the time since the last loop
329 : // 3. using the desired attitude and input variables, define the target angular velocity so that it should
330 : // move the target attitude towards the desired attitude
331 : // 4. if _rate_bf_ff_enabled is not being used then make the target attitude
332 : // and target angular velocities equal to the desired attitude and desired angular velocities.
333 : // 5. ensure _attitude_target, _euler_angle_target_rad, _euler_rate_target_rads and
334 : // _ang_vel_target_rads have been defined. This ensures input modes can be changed without discontinuity.
335 : // 6. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
336 : // integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
337 : // corrected by first correcting the thrust vector until the angle between the target thrust vector measured
338 : // trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD. At this point the heading is also corrected.
339 :
340 : // Sets an attitude target using a quaternion and a body-frame angular velocity input (rad/s).
341 : // The desired quaternion is incrementally advanced each timestep using the (limited) angular velocity input.
342 : // If body-frame rate feedforward shaping is enabled, rate/accel targets are generated with acceleration limits
343 : // and input time constants; otherwise the targets are set directly.
344 98513 : void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body_rads)
345 : {
346 : // Update internal attitude target state
347 98513 : update_attitude_target();
348 :
349 : // Limit the body-frame angular velocity input
350 98513 : ang_vel_limit(ang_vel_body_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs));
351 :
352 : // Convert the limited body-frame angular velocity input into the frame used for quaternion integration
353 98513 : Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body_rads;
354 :
355 98513 : if (_rate_bf_ff_enabled) {
356 : // Compute attitude error (target -> desired) and express as a small-axis-angle vector
357 98513 : Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
358 98513 : Vector3f attitude_error_angle;
359 98513 : attitude_error_quat.to_axis_angle(attitude_error_angle);
360 :
361 : // Shape the attitude error into angular velocity and acceleration targets with configured
362 : // rate and acceleration limits and time constants (roll/pitch use _input_tc, yaw uses _rate_y_tc).
363 98513 : attitude_command_model(wrap_PI(attitude_error_angle.x), 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s);
364 98513 : attitude_command_model(wrap_PI(attitude_error_angle.y), 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s);
365 98513 : attitude_command_model(wrap_PI(attitude_error_angle.z), 0.0, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
366 : } else {
367 : // No shaping: directly set attitude and angular velocity targets
368 0 : _attitude_target = attitude_desired_quat;
369 0 : _ang_vel_target_rads = ang_vel_target;
370 : }
371 :
372 : // Update stored Euler-angle representation of the attitude target
373 98513 : _attitude_target.to_euler(_euler_angle_target_rad);
374 :
375 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
376 98513 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
377 :
378 : // Incrementally advance the desired quaternion using the (limited) angular velocity input
379 98513 : Quaternion attitude_desired_update;
380 98513 : attitude_desired_update.from_axis_angle(ang_vel_target * _dt_s);
381 98513 : attitude_desired_quat = attitude_desired_quat * attitude_desired_update;
382 98513 : attitude_desired_quat.normalize();
383 :
384 : // Run quaternion attitude controller
385 98513 : attitude_controller_run_quat();
386 98513 : }
387 :
388 : // Sets the desired roll and pitch angles (in centidegrees) and yaw rate (in centidegrees/s).
389 : // See input_euler_angle_roll_pitch_euler_rate_yaw_rad() for full details.
390 2228903 : void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
391 : {
392 : // Convert from centidegrees on public interface to radians
393 2228903 : const float euler_roll_angle_rad = cd_to_rad(euler_roll_angle_cd);
394 2228903 : const float euler_pitch_angle_rad = cd_to_rad(euler_pitch_angle_cd);
395 2228903 : const float euler_yaw_rate_rads = cd_to_rad(euler_yaw_rate_cds);
396 2228903 : input_euler_angle_roll_pitch_euler_rate_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_rate_rads);
397 2228903 : }
398 :
399 : // Sets the desired roll and pitch angle inputs (radians) and a yaw rate input (radians/s).
400 : // Used when roll/pitch stabilization is required while yaw is controlled as a rate.
401 : // If body-frame rate feedforward shaping is enabled, shapes Euler rate/acceleration targets
402 : // with configured limits and time constants and converts them back to body-frame targets.
403 : // Otherwise, updates the attitude target directly and zeros rate/accel feedforward targets.
404 15121138 : void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads)
405 : {
406 : // update attitude target
407 15121138 : update_attitude_target();
408 :
409 : // calculate the attitude target euler angles
410 15121138 : _attitude_target.to_euler(_euler_angle_target_rad);
411 :
412 : // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
413 15121138 : euler_roll_angle_rad += get_roll_trim_rad();
414 :
415 15121138 : if (_rate_bf_ff_enabled) {
416 : // Convert body-frame angular acceleration limits (roll, pitch, yaw) into equivalent
417 : // Euler-angle acceleration limits for the current attitude target.
418 14875815 : const Vector3f euler_accel_radss = body_to_euler_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
419 14875815 : const Vector3f euler_rate_max_rads = body_to_euler_limit(_attitude_target, Vector3f{radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)});
420 :
421 : // Convert the body-frame angular acceleration target into an equivalent Euler-angle
422 : // acceleration target for the current attitude target.
423 14875815 : Vector3f euler_accel_target_rads;
424 14875815 : body_to_euler_derivative(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads);
425 :
426 : // Shape Euler roll/pitch angle error into Euler rate/acceleration targets.
427 : // The shaper applies Euler rate/acceleration limits and the configured input time constant.
428 14875815 : attitude_command_model(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), 0.0, _euler_rate_target_rads.x, euler_accel_target_rads.x, fabsf(euler_rate_max_rads.x), euler_accel_radss.x, _input_tc, _dt_s);
429 14875815 : attitude_command_model(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), 0.0, _euler_rate_target_rads.y, euler_accel_target_rads.y, fabsf(euler_rate_max_rads.y), euler_accel_radss.y, _input_tc, _dt_s);
430 :
431 : // Shape yaw rate input into Euler yaw rate/acceleration targets, applying the configured yaw rate time constant
432 : // and limiting Euler acceleration about the yaw axis.
433 14875815 : attitude_command_model(0.0, euler_yaw_rate_rads, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(euler_rate_max_rads.z), euler_accel_radss.z, _rate_y_tc, _dt_s);
434 :
435 : // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
436 14875815 : euler_derivative_to_body(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads);
437 :
438 : // Convert euler angle derivative of desired attitude into a body-frame angular acceleration vector for feedforward
439 14875815 : euler_derivative_to_body(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads);
440 : } else {
441 : // No shaping/feedforward: directly update roll/pitch attitude targets and integrate yaw target from yaw rate.
442 245323 : _euler_angle_target_rad.x = euler_roll_angle_rad;
443 245323 : _euler_angle_target_rad.y = euler_pitch_angle_rad;
444 245323 : _euler_angle_target_rad.z += euler_yaw_rate_rads * _dt_s;
445 :
446 : // Compute quaternion target attitude
447 245323 : _attitude_target.from_euler(_euler_angle_target_rad.x, _euler_angle_target_rad.y, _euler_angle_target_rad.z);
448 :
449 : // Zero rate and acceleration feedforward targets
450 245323 : _euler_rate_target_rads.zero();
451 245323 : _ang_vel_target_rads.zero();
452 245323 : _ang_accel_target_rads.zero();
453 : }
454 :
455 : // Call quaternion attitude controller
456 15121138 : attitude_controller_run_quat();
457 15121138 : }
458 :
459 : // Sets the desired roll, pitch, and yaw angles (in centidegrees).
460 : // See input_euler_angle_roll_pitch_yaw_rad() for full details.
461 965469 : void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
462 : {
463 : // Convert from centidegrees on public interface to radians
464 965469 : const float euler_roll_angle_rad = cd_to_rad(euler_roll_angle_cd);
465 965469 : const float euler_pitch_angle_rad = cd_to_rad(euler_pitch_angle_cd);
466 965469 : const float euler_yaw_angle_rad = cd_to_rad(euler_yaw_angle_cd);
467 :
468 965469 : input_euler_angle_roll_pitch_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_angle_rad, slew_yaw);
469 965469 : }
470 :
471 : // Sets the desired roll, pitch, and yaw angle inputs (radians) to follow an absolute attitude setpoint.
472 : // Optional yaw slew limiting constrains the rate of change of the yaw target.
473 : // If body-frame rate feedforward shaping is enabled, shapes Euler rate/acceleration targets
474 : // with configured limits and time constants and converts them back to body-frame targets.
475 : // Otherwise, updates the attitude target directly (with optional yaw slew) and zeros rate/accel feedforward targets.
476 2183113 : void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_angle_rad, bool slew_yaw)
477 : {
478 : // update attitude target
479 2183113 : update_attitude_target();
480 :
481 : // calculate the attitude target euler angles
482 2183113 : _attitude_target.to_euler(_euler_angle_target_rad);
483 :
484 : // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
485 2183113 : euler_roll_angle_rad += get_roll_trim_rad();
486 :
487 2183113 : float yaw_rate_max_rads = radians(_ang_vel_yaw_max_degs);
488 2183113 : if (slew_yaw) {
489 : // Replace global yaw rate limit with the slew rate limit (0 disables limiting).
490 1895898 : yaw_rate_max_rads = get_slew_yaw_max_rads();
491 : }
492 :
493 2183113 : if (_rate_bf_ff_enabled) {
494 : // Convert body-frame angular rate and acceleration limits (roll, pitch, yaw) into equivalent
495 : // Euler-angle rate and acceleration limits for the current attitude target.
496 1905445 : const Vector3f euler_accel_radss = body_to_euler_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
497 1905445 : const Vector3f euler_rate_max_rads = body_to_euler_limit(_attitude_target, Vector3f{radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), yaw_rate_max_rads});
498 :
499 1905445 : Vector3f euler_accel_target_rads;
500 1905445 : body_to_euler_derivative(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads);
501 1905445 : attitude_command_model(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), 0.0, _euler_rate_target_rads.x, euler_accel_target_rads.x, fabsf(euler_rate_max_rads.x), euler_accel_radss.x, _input_tc, _dt_s);
502 1905445 : attitude_command_model(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), 0.0, _euler_rate_target_rads.y, euler_accel_target_rads.y, fabsf(euler_rate_max_rads.y), euler_accel_radss.y, _input_tc, _dt_s);
503 1905445 : attitude_command_model(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), 0.0, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(euler_rate_max_rads.z), euler_accel_radss.z, _input_tc, _dt_s);
504 :
505 : // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
506 1905445 : euler_derivative_to_body(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads);
507 :
508 : // Convert euler angle derivative of desired attitude into a body-frame angular acceleration vector for feedforward
509 1905445 : euler_derivative_to_body(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads);
510 : } else {
511 : // No shaping/feedforward: directly update roll/pitch attitude targets and update yaw target
512 : // with optional slew limiting, then zero rate/accel feedforward targets.
513 277668 : _euler_angle_target_rad.x = euler_roll_angle_rad;
514 277668 : _euler_angle_target_rad.y = euler_pitch_angle_rad;
515 :
516 277668 : if (is_positive(yaw_rate_max_rads)) {
517 : // Constrain yaw target change to the configured slew rate.
518 127 : const float yaw_error = wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z);
519 127 : const float yaw_step = constrain_float(yaw_error, -yaw_rate_max_rads * _dt_s, yaw_rate_max_rads * _dt_s);
520 127 : _euler_angle_target_rad.z = wrap_PI(_euler_angle_target_rad.z + yaw_step);
521 : } else {
522 277541 : _euler_angle_target_rad.z = euler_yaw_angle_rad;
523 : }
524 :
525 : // Compute quaternion target attitude
526 277668 : _attitude_target.from_euler(_euler_angle_target_rad.x, _euler_angle_target_rad.y, _euler_angle_target_rad.z);
527 :
528 : // Zero rate and acceleration feedforward targets
529 277668 : _euler_rate_target_rads.zero();
530 277668 : _ang_vel_target_rads.zero();
531 277668 : _ang_accel_target_rads.zero();
532 : }
533 :
534 : // Call quaternion attitude controller
535 2183113 : attitude_controller_run_quat();
536 2183113 : }
537 :
538 : // Sets the desired roll, pitch, and yaw Euler angle rate inputs (radians/s).
539 : // If body-frame rate feedforward shaping is enabled, the inputs are shaped using acceleration limits
540 : // and time constants to generate Euler rate/acceleration targets, which are then converted into
541 : // body-frame angular velocity/acceleration targets for the rate controller.
542 : // Otherwise, Euler angle targets are integrated directly from the rate inputs and feedforward targets are zeroed.
543 0 : void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_rads(float euler_roll_rate_rads, float euler_pitch_rate_rads, float euler_yaw_rate_rads)
544 : {
545 : // update attitude target
546 0 : update_attitude_target();
547 :
548 : // calculate the attitude target euler angles
549 0 : _attitude_target.to_euler(_euler_angle_target_rad);
550 :
551 0 : if (_rate_bf_ff_enabled) {
552 : // Convert body-frame angular acceleration limits (roll, pitch, yaw) into
553 : // equivalent Euler-angle acceleration limits for the current attitude target.
554 0 : const Vector3f euler_accel_radss = body_to_euler_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
555 :
556 : // Convert the body-frame angular acceleration target into an equivalent Euler-angle
557 : // acceleration target for the current attitude target.
558 0 : Vector3f euler_accel_target_rads;
559 0 : body_to_euler_derivative(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads);
560 :
561 : // Shape Euler rate inputs into Euler rate/acceleration targets, applying acceleration limits and time constants.
562 0 : attitude_command_model(0.0, euler_roll_rate_rads, _euler_rate_target_rads.x, euler_accel_target_rads.x, 0.0, euler_accel_radss.x, _rate_rp_tc, _dt_s);
563 0 : attitude_command_model(0.0, euler_pitch_rate_rads, _euler_rate_target_rads.y, euler_accel_target_rads.y, 0.0, euler_accel_radss.y, _rate_rp_tc, _dt_s);
564 0 : attitude_command_model(0.0, euler_yaw_rate_rads, _euler_rate_target_rads.z, euler_accel_target_rads.z, 0.0, euler_accel_radss.z, _rate_y_tc, _dt_s);
565 :
566 : // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
567 0 : euler_derivative_to_body(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads);
568 :
569 : // Convert euler angle derivative of desired attitude into a body-frame angular acceleration vector for feedforward
570 0 : euler_derivative_to_body(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads);
571 : } else {
572 : // No shaping/feedforward: integrate Euler angle targets from Euler rate inputs.
573 : // Pitch is constrained to ±85 degrees to avoid gimbal lock discontinuities.
574 0 : _euler_angle_target_rad.x = wrap_PI(_euler_angle_target_rad.x + euler_roll_rate_rads * _dt_s);
575 0 : _euler_angle_target_rad.y = constrain_float(_euler_angle_target_rad.y + euler_pitch_rate_rads * _dt_s, radians(-85.0f), radians(85.0f));
576 0 : _euler_angle_target_rad.z = wrap_2PI(_euler_angle_target_rad.z + euler_yaw_rate_rads * _dt_s);
577 :
578 : // Zero rate and acceleration feedforward targets
579 0 : _euler_rate_target_rads.zero();
580 0 : _ang_vel_target_rads.zero();
581 0 : _ang_accel_target_rads.zero();
582 :
583 : // Compute quaternion target attitude
584 0 : _attitude_target.from_euler(_euler_angle_target_rad.x, _euler_angle_target_rad.y, _euler_angle_target_rad.z);
585 : }
586 :
587 : // Call quaternion attitude controller
588 0 : attitude_controller_run_quat();
589 0 : }
590 :
591 : // Fully stabilized acro
592 : // Sets the desired roll, pitch, and yaw angular rates (in centidegrees/s).
593 : // See input_rate_bf_roll_pitch_yaw_rads() for full details.
594 360 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
595 : {
596 : // Convert from centidegrees on public interface to radians
597 360 : const float roll_rate_bf_rads = cd_to_rad(roll_rate_bf_cds);
598 360 : const float pitch_rate_bf_rads = cd_to_rad(pitch_rate_bf_cds);
599 360 : const float yaw_rate_bf_rads = cd_to_rad(yaw_rate_bf_cds);
600 :
601 360 : input_rate_bf_roll_pitch_yaw_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
602 360 : }
603 :
604 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
605 : // Used by fully stabilized acro modes.
606 : // If body-frame rate feedforward shaping is enabled, the inputs are shaped using acceleration limits
607 : // and time constants to generate body-frame angular velocity/acceleration targets for the rate controller.
608 : // Otherwise, the attitude target is incrementally rotated using the rate inputs and feedforward targets are zeroed.
609 113716 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads)
610 : {
611 : // update attitude target
612 113716 : update_attitude_target();
613 :
614 : // calculate the attitude target euler angles
615 113716 : _attitude_target.to_euler(_euler_angle_target_rad);
616 :
617 113716 : if (_rate_bf_ff_enabled) {
618 : // Shape body-frame rate inputs into body-frame angular velocity/acceleration targets,
619 : // applying acceleration limits and configured time constants.
620 113716 : attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s);
621 113716 : attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s);
622 113716 : attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
623 :
624 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
625 113716 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
626 : } else {
627 : // No shaping/feedforward: incrementally rotate the attitude target using the body-frame rate inputs.
628 0 : Quaternion attitude_target_update;
629 0 : attitude_target_update.from_axis_angle(Vector3f{roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads} * _dt_s);
630 0 : _attitude_target = _attitude_target * attitude_target_update;
631 0 : _attitude_target.normalize();
632 :
633 : // Zero rate and acceleration feedforward targets
634 0 : _euler_rate_target_rads.zero();
635 0 : _ang_vel_target_rads.zero();
636 0 : _ang_accel_target_rads.zero();
637 : }
638 :
639 : // Call quaternion attitude controller
640 113716 : attitude_controller_run_quat();
641 113716 : }
642 :
643 : // Sets the desired roll, pitch, and yaw angular rates in body-frame (in centidegrees/s).
644 : // See input_rate_bf_roll_pitch_yaw_2_rads() for full details.
645 9994 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
646 : {
647 : // Convert from centidegrees on public interface to radians
648 9994 : const float roll_rate_bf_rads = cd_to_rad(roll_rate_bf_cds);
649 9994 : const float pitch_rate_bf_rads = cd_to_rad(pitch_rate_bf_cds);
650 9994 : const float yaw_rate_bf_rads = cd_to_rad(yaw_rate_bf_cds);
651 :
652 9994 : input_rate_bf_roll_pitch_yaw_2_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
653 9994 : }
654 :
655 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
656 : // Used by Copter's rate-only acro mode.
657 : // Shapes the body-frame rate inputs using acceleration limits and time constants to produce
658 : // body-frame angular velocity/acceleration targets for the rate controller.
659 : // Attitude targets are updated from the current AHRS attitude to keep target state coherent for mode transitions.
660 9994 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads)
661 : {
662 : // Shape body-frame rate inputs into body-frame angular velocity/acceleration targets,
663 : // applying acceleration limits and configured time constants.
664 9994 : attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s);
665 9994 : attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s);
666 9994 : attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
667 :
668 : // Update attitude and Euler targets from the current vehicle attitude (used for conditioning mode transitions).
669 9994 : _ahrs.get_quat_body_to_ned(_attitude_target);
670 9994 : _attitude_target.to_euler(_euler_angle_target_rad);
671 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
672 9994 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
673 :
674 : // Update body-frame angular velocity target used by the rate controller.
675 9994 : _ang_vel_body_rads = _ang_vel_target_rads;
676 9994 : }
677 :
678 : // Sets the desired roll, pitch, and yaw angular rates in body-frame (in centidegrees/s).
679 : // See input_rate_bf_roll_pitch_yaw_3_rads() for full details.
680 0 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
681 : {
682 : // Convert from centidegrees on public interface to radians
683 0 : const float roll_rate_bf_rads = cd_to_rad(roll_rate_bf_cds);
684 0 : const float pitch_rate_bf_rads = cd_to_rad(pitch_rate_bf_cds);
685 0 : const float yaw_rate_bf_rads = cd_to_rad(yaw_rate_bf_cds);
686 :
687 0 : input_rate_bf_roll_pitch_yaw_3_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
688 0 : }
689 :
690 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
691 : // Used by Plane's acro mode with rate error integration.
692 : // Maintains an integrated attitude error quaternion using (target_rate - gyro) and combines it
693 : // with shaped rate inputs to form the final body-frame angular velocity target for the rate controller.
694 0 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads)
695 : {
696 : // Extract current integrated attitude error as axis-angle
697 0 : Vector3f attitude_error;
698 0 : _attitude_ang_error.to_axis_angle(attitude_error);
699 :
700 0 : Quaternion attitude_ang_error_update_quat;
701 :
702 : // Limit the magnitude of the integrated attitude error (prevents windup)
703 0 : float err_mag = attitude_error.length();
704 0 : if (err_mag > AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD) {
705 0 : attitude_error *= AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD / err_mag;
706 0 : _attitude_ang_error.from_axis_angle(attitude_error);
707 : }
708 :
709 : // Integrate attitude error using rate error: (target body rates - measured gyro)
710 0 : Vector3f gyro_latest = get_latest_gyro();
711 0 : attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target_rads - gyro_latest) * _dt_s);
712 0 : _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
713 0 : _attitude_ang_error.normalize();
714 :
715 : // Shape body-frame rate inputs into body-frame angular velocity/acceleration targets,
716 : // applying acceleration limits and configured time constants.
717 0 : attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s);
718 0 : attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s);
719 0 : attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
720 :
721 : // Retrieve current vehicle attitude (body to NED)
722 0 : Quaternion attitude_body;
723 0 : _ahrs.get_quat_body_to_ned(attitude_body);
724 :
725 : // Update attitude target by applying the integrated attitude error to the current attitude
726 0 : _attitude_target = attitude_body * _attitude_ang_error;
727 0 : _attitude_target.normalize();
728 :
729 : // Update stored Euler-angle representation of the attitude target
730 0 : _attitude_target.to_euler(_euler_angle_target_rad);
731 :
732 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
733 0 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
734 :
735 : // Compute body-frame angular velocity correction from integrated attitude error and add shaped rate input
736 0 : _attitude_ang_error.to_axis_angle(attitude_error);
737 0 : Vector3f ang_vel_body_rads = update_ang_vel_target_from_att_error(attitude_error);
738 0 : ang_vel_body_rads += _ang_vel_target_rads;
739 :
740 : // Update body-frame angular velocity target used by the rate controller
741 0 : _ang_vel_body_rads = ang_vel_body_rads;
742 0 : }
743 :
744 : /*
745 : set the body frame target rates to the specified rates, used by the
746 : quadplane code when we want to slave the VTOL controller rates to
747 : the fixed wing rates
748 : */
749 :
750 : // Directly sets the body-frame angular rates without smoothing (in centidegrees/s).
751 : // See input_rate_bf_roll_pitch_yaw_no_shaping_rads() for full details.
752 414050 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_no_shaping_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
753 : {
754 : // Convert from centidegrees on public interface to radians
755 414050 : const float roll_rate_bf_rads = cd_to_rad(roll_rate_bf_cds);
756 414050 : const float pitch_rate_bf_rads = cd_to_rad(pitch_rate_bf_cds);
757 414050 : const float yaw_rate_bf_rads = cd_to_rad(yaw_rate_bf_cds);
758 :
759 414050 : input_rate_bf_roll_pitch_yaw_no_shaping_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
760 414050 : }
761 :
762 : // Directly sets body-frame angular rate targets (radians/s) without shaping.
763 : // Used when an external controller (e.g. fixed-wing controller) provides VTOL body rates.
764 : // No smoothing, acceleration limiting, or input shaping is applied.
765 414050 : void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_no_shaping_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads)
766 : {
767 : // Set body-frame angular velocity targets directly from inputs
768 414050 : _ang_vel_target_rads.x = roll_rate_bf_rads;
769 414050 : _ang_vel_target_rads.y = pitch_rate_bf_rads;
770 414050 : _ang_vel_target_rads.z = yaw_rate_bf_rads;
771 :
772 : // Update attitude and Euler targets from the current vehicle attitude
773 : // (used for mode transitions / logging / target state consistency).
774 414050 : _ahrs.get_quat_body_to_ned(_attitude_target);
775 414050 : _attitude_target.to_euler(_euler_angle_target_rad);
776 :
777 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
778 414050 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
779 :
780 : // Update body-frame angular velocity target used by the rate controller.
781 414050 : _ang_vel_body_rads = _ang_vel_target_rads;
782 414050 : }
783 :
784 : // Applies a one-time angular offset to the attitude target using body-frame roll, pitch,
785 : // and yaw angles (radians).
786 : // Used for step-response excitation during autotuning or manual test inputs.
787 : // The offset is applied instantaneously; no rate or acceleration shaping is performed.
788 471 : void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw_rad(float roll_angle_step_bf_rad, float pitch_angle_step_bf_rad, float yaw_angle_step_bf_rad)
789 : {
790 : // Apply the requested body-frame angular step to the attitude target
791 471 : Quaternion attitude_target_update;
792 471 : attitude_target_update.from_axis_angle(Vector3f{roll_angle_step_bf_rad, pitch_angle_step_bf_rad, yaw_angle_step_bf_rad});
793 471 : _attitude_target = _attitude_target * attitude_target_update;
794 471 : _attitude_target.normalize();
795 :
796 : // Update stored Euler-angle representation of the attitude target
797 471 : _attitude_target.to_euler(_euler_angle_target_rad);
798 :
799 : // Zero rate and acceleration feedforward targets (pure attitude step)
800 471 : _euler_rate_target_rads.zero();
801 471 : _ang_vel_target_rads.zero();
802 471 : _ang_accel_target_rads.zero();
803 :
804 : // Run quaternion attitude controller
805 471 : attitude_controller_run_quat();
806 471 : }
807 :
808 : // Applies a one-time body-frame angular rate step (radians/s) in roll, pitch, and yaw.
809 : // Used to inject discrete disturbances or step inputs for system identification.
810 : // This sets the body-frame rate command directly for this update; no shaping is applied.
811 210919 : void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw_rads(float roll_rate_step_bf_rads, float pitch_rate_step_bf_rads, float yaw_rate_step_bf_rads)
812 : {
813 : // Update attitude and Euler targets from the current vehicle attitude
814 : // (used for mode transitions / logging / target state consistency).
815 210919 : _ahrs.get_quat_body_to_ned(_attitude_target);
816 210919 : _attitude_target.to_euler(_euler_angle_target_rad);
817 :
818 : // Zero rate and acceleration feedforward targets so the controller cleanly returns to zero rate
819 : // after the step input is removed.
820 210919 : _ang_vel_target_rads.zero();
821 210919 : _ang_accel_target_rads.zero();
822 210919 : _euler_rate_target_rads.zero();
823 :
824 : // Apply the requested body-frame angular rate step directly to the rate controller input.
825 210919 : _ang_vel_body_rads = Vector3f{roll_rate_step_bf_rads, pitch_rate_step_bf_rads, yaw_rate_step_bf_rads};
826 210919 : }
827 :
828 : // Sets the desired thrust vector and a yaw/heading rate input (radians/s).
829 : // Used for tilt-based navigation with independent yaw control.
830 : // The thrust vector determines the desired tilt (attitude) while the heading rate commands yaw.
831 : // Optional yaw slew limiting constrains the heading rate input.
832 12695991 : void AC_AttitudeControl::input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw)
833 : {
834 12695991 : float yaw_rate_max_rads = radians(_ang_vel_yaw_max_degs);
835 12695991 : if (slew_yaw) {
836 : // Replace global yaw rate limit with the slew rate limit (0 disables limiting).
837 9236490 : yaw_rate_max_rads = get_slew_yaw_max_rads();
838 : }
839 :
840 : // update attitude target
841 12695991 : update_attitude_target();
842 :
843 : // calculate the attitude target euler angles
844 12695991 : _attitude_target.to_euler(_euler_angle_target_rad);
845 :
846 : // Convert thrust vector to an attitude quaternion (zero yaw; yaw is commanded separately).
847 12695991 : Quaternion thrust_vec_quat = attitude_from_thrust_vector(thrust_vector, 0.0f);
848 :
849 : // Compute the attitude correction required to align the current target attitude with the thrust vector.
850 : float thrust_vector_diff_angle;
851 12695991 : Quaternion thrust_vec_correction_quat;
852 12695991 : Vector3f attitude_error;
853 : float returned_thrust_vector_angle;
854 12695991 : thrust_vector_rotation_angles(thrust_vec_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
855 :
856 12695991 : if (_rate_bf_ff_enabled) {
857 : // Shape roll/pitch attitude error into body-frame angular velocity/acceleration targets,
858 : // applying configured rate/acceleration limits and input time constant.
859 12695991 : attitude_command_model(attitude_error.x, 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s);
860 12695991 : attitude_command_model(attitude_error.y, 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s);
861 :
862 : // Shape yaw rate input into yaw angular velocity/acceleration targets, applying yaw limits and time constant.
863 12695991 : attitude_command_model(0.0, heading_rate_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, yaw_rate_max_rads, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
864 : } else {
865 : // No shaping/feedforward: directly update the attitude target using the thrust-vector correction and yaw increment.
866 0 : if (is_positive(yaw_rate_max_rads)) {
867 0 : heading_rate_rads = constrain_float(heading_rate_rads, -yaw_rate_max_rads, yaw_rate_max_rads);
868 : }
869 0 : Quaternion yaw_quat;
870 0 : yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate_rads * _dt_s});
871 0 : _attitude_target = _attitude_target * thrust_vec_correction_quat * yaw_quat;
872 0 : _attitude_target.normalize();
873 :
874 : // Zero rate and acceleration feedforward targets
875 0 : _euler_rate_target_rads.zero();
876 0 : _ang_vel_target_rads.zero();
877 0 : _ang_accel_target_rads.zero();
878 : }
879 :
880 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
881 12695991 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
882 :
883 : // Call quaternion attitude controller
884 12695991 : attitude_controller_run_quat();
885 12695991 : }
886 :
887 : // Sets the desired thrust vector, heading angle (radians), and heading rate input (radians/s).
888 : // Used when thrust direction (tilt) is commanded independently from yaw/heading.
889 : // Heading rate is constrained using the configured yaw slew rate limit (0 disables limiting).
890 2743232 : void AC_AttitudeControl::input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads)
891 : {
892 : // update attitude target
893 2743232 : update_attitude_target();
894 :
895 : // calculate the attitude target euler angles
896 2743232 : _attitude_target.to_euler(_euler_angle_target_rad);
897 :
898 : // Convert thrust vector and heading into a desired attitude quaternion.
899 2743232 : const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle_rad);
900 :
901 2743232 : if (_rate_bf_ff_enabled) {
902 : // Compute attitude error required to move from the current target attitude to the desired attitude.
903 2743232 : Vector3f attitude_error;
904 : float thrust_vector_diff_angle;
905 2743232 : Quaternion thrust_vec_correction_quat;
906 : float returned_thrust_vector_angle;
907 2743232 : thrust_vector_rotation_angles(desired_attitude_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
908 :
909 : // Shape attitude error and heading rate into body-frame angular velocity/acceleration targets,
910 : // applying configured rate/acceleration limits and time constants (roll/pitch use _input_tc, yaw uses _rate_y_tc).
911 2743232 : attitude_command_model(attitude_error.x, 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s);
912 2743232 : attitude_command_model(attitude_error.y, 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s);
913 2743232 : attitude_command_model(attitude_error.z, heading_rate_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, get_slew_yaw_max_rads(), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s);
914 : } else {
915 : // No shaping/feedforward: directly set the attitude target to the desired attitude.
916 0 : _attitude_target = desired_attitude_quat;
917 :
918 : // Zero rate and acceleration feedforward targets
919 0 : _euler_rate_target_rads.zero();
920 0 : _ang_vel_target_rads.zero();
921 0 : _ang_accel_target_rads.zero();
922 : }
923 :
924 : // Convert body-frame angular velocity into euler angle derivative of desired attitude
925 2743232 : body_to_euler_derivative(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);
926 :
927 : // Call quaternion attitude controller
928 2743232 : attitude_controller_run_quat();
929 2743232 : }
930 :
931 : // Command a thrust vector and heading rate
932 11730957 : void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading)
933 : {
934 11730957 : switch (heading.heading_mode) {
935 8996498 : case HeadingMode::Rate_Only:
936 8996498 : input_thrust_vector_rate_heading_rads(thrust_vector, heading.yaw_rate_rads);
937 8996498 : break;
938 0 : case HeadingMode::Angle_Only:
939 0 : input_thrust_vector_heading_rad(thrust_vector, heading.yaw_angle_rad, 0.0);
940 0 : break;
941 2734459 : case HeadingMode::Angle_And_Rate:
942 2734459 : input_thrust_vector_heading_rad(thrust_vector, heading.yaw_angle_rad, heading.yaw_rate_rads);
943 2734459 : break;
944 : }
945 11730957 : }
946 :
947 15470758 : Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle_rad) const
948 : {
949 15470758 : const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
950 :
951 15470758 : if (is_zero(thrust_vector.length_squared())) {
952 0 : thrust_vector = thrust_vector_up;
953 : } else {
954 15470758 : thrust_vector.normalize();
955 : }
956 :
957 : // the cross product of the desired and target thrust vector defines the rotation vector
958 15470758 : Vector3f thrust_vec_cross = thrust_vector_up % thrust_vector;
959 :
960 : // the dot product is used to calculate the angle between the target and desired thrust vectors
961 15470758 : const float thrust_vector_angle = acosf(constrain_float(thrust_vector_up * thrust_vector, -1.0f, 1.0f));
962 :
963 : // Normalize the thrust rotation vector
964 15470758 : const float thrust_vector_length = thrust_vec_cross.length();
965 15470758 : if (is_zero(thrust_vector_length) || is_zero(thrust_vector_angle)) {
966 1672188 : thrust_vec_cross = thrust_vector_up;
967 : } else {
968 13798570 : thrust_vec_cross /= thrust_vector_length;
969 : }
970 :
971 15470758 : Quaternion thrust_vec_quat;
972 15470758 : thrust_vec_quat.from_axis_angle(thrust_vec_cross, thrust_vector_angle);
973 15470758 : Quaternion yaw_quat;
974 15470758 : yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, 1.0f}, heading_angle_rad);
975 15470758 : return thrust_vec_quat*yaw_quat;
976 : }
977 :
978 : // Calculates the body frame angular velocities to follow the target attitude
979 32955703 : void AC_AttitudeControl::update_attitude_target()
980 : {
981 : // rotate target and normalize
982 32955703 : Quaternion attitude_target_update;
983 32955703 : attitude_target_update.from_axis_angle(_ang_vel_target_rads * _dt_s);
984 32955703 : _attitude_target *= attitude_target_update;
985 32955703 : _attitude_target.normalize();
986 32955703 : }
987 :
988 : // Calculates the body frame angular velocities to follow the target attitude
989 32956174 : void AC_AttitudeControl::attitude_controller_run_quat()
990 : {
991 : // This represents a quaternion rotation in NED frame to the body
992 32956174 : Quaternion attitude_body;
993 32956174 : _ahrs.get_quat_body_to_ned(attitude_body);
994 :
995 : // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
996 32956174 : Vector3f attitude_error;
997 32956174 : thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle_rad, _thrust_error_angle_rad);
998 :
999 : // Compute the angular velocity corrections in the body frame from the attitude error
1000 32956174 : Vector3f ang_vel_body_rads = update_ang_vel_target_from_att_error(attitude_error);
1001 :
1002 : // ensure angular velocity does not go over configured limits
1003 32956174 : ang_vel_limit(ang_vel_body_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs));
1004 :
1005 : // rotation from the target frame to the body frame
1006 32956174 : Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
1007 :
1008 : // target angle velocity vector in the body frame
1009 32956174 : Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target_rads;
1010 32956174 : Vector3f gyro = get_latest_gyro();
1011 : // Correct the thrust vector and smoothly add feedforward and yaw input
1012 32956174 : _feedforward_scalar = 1.0f;
1013 32956174 : if (_thrust_error_angle_rad > AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD * 2.0f) {
1014 505361 : ang_vel_body_rads.z = gyro.z;
1015 32450813 : } else if (_thrust_error_angle_rad > AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD) {
1016 135950 : _feedforward_scalar = (1.0f - (_thrust_error_angle_rad - AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD) / AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD);
1017 135950 : ang_vel_body_rads.x += ang_vel_body_feedforward.x * _feedforward_scalar;
1018 135950 : ang_vel_body_rads.y += ang_vel_body_feedforward.y * _feedforward_scalar;
1019 135950 : ang_vel_body_rads.z += ang_vel_body_feedforward.z;
1020 135950 : ang_vel_body_rads.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body_rads.z * _feedforward_scalar;
1021 : } else {
1022 32314863 : ang_vel_body_rads += ang_vel_body_feedforward;
1023 : }
1024 :
1025 : // Record error to handle EKF resets
1026 32956174 : _attitude_ang_error = attitude_body.inverse() * _attitude_target;
1027 : // finally update the attitude target
1028 32956174 : _ang_vel_body_rads = ang_vel_body_rads;
1029 32956174 : }
1030 :
1031 : // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
1032 : // The maximum error in the yaw axis is limited based on static output saturation.
1033 32964574 : void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error_rad, float& thrust_angle_rad, float& thrust_error_angle_rad) const
1034 : {
1035 32964574 : Quaternion thrust_vector_correction;
1036 32964574 : thrust_vector_rotation_angles(attitude_target, attitude_body, thrust_vector_correction, attitude_error_rad, thrust_angle_rad, thrust_error_angle_rad);
1037 :
1038 : // Todo: Limit roll an pitch error based on output saturation and maximum error.
1039 :
1040 : // Limit Yaw Error based to the maximum that would saturate the output when yaw rate is zero.
1041 32964574 : Quaternion heading_vec_correction_quat;
1042 :
1043 32964574 : float heading_accel_max = constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS);
1044 32964574 : if (!is_zero(get_rate_yaw_pid().kP())) {
1045 32964574 : float heading_error_max = MIN(inv_sqrt_controller(1.0 / get_rate_yaw_pid().kP(), _p_angle_yaw.kP(), heading_accel_max), AC_ATTITUDE_YAW_MAX_ERROR_ANGLE_RAD);
1046 32964574 : if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error_rad.z) > heading_error_max) {
1047 401605 : attitude_error_rad.z = constrain_float(wrap_PI(attitude_error_rad.z), -heading_error_max, heading_error_max);
1048 401605 : heading_vec_correction_quat.from_axis_angle(Vector3f{0.0f, 0.0f, attitude_error_rad.z});
1049 401605 : attitude_target = attitude_body * thrust_vector_correction * heading_vec_correction_quat;
1050 : }
1051 : }
1052 32964574 : }
1053 :
1054 : // thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
1055 : // The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
1056 48403797 : void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error_rad, float& thrust_angle_rad, float& thrust_error_angle_rad) const
1057 : {
1058 : // The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
1059 48403797 : const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
1060 :
1061 : // attitude_target and attitude_body are passive rotations from target / body frames to the NED frame
1062 :
1063 : // Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
1064 48403797 : const Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
1065 :
1066 : // Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
1067 48403797 : const Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up; // current thrust vector
1068 :
1069 : // the dot product is used to calculate the current lean angle for use of external functions
1070 48403797 : thrust_angle_rad = acosf(constrain_float(thrust_vector_up * att_body_thrust_vec,-1.0f,1.0f));
1071 :
1072 : // the cross product of the desired and target thrust vector defines the rotation vector
1073 48403797 : Vector3f thrust_vec_cross = att_body_thrust_vec % att_target_thrust_vec;
1074 :
1075 : // the dot product is used to calculate the angle between the target and desired thrust vectors
1076 48403797 : thrust_error_angle_rad = acosf(constrain_float(att_body_thrust_vec * att_target_thrust_vec, -1.0f, 1.0f));
1077 :
1078 : // Normalize the thrust rotation vector
1079 48403797 : float thrust_vector_length = thrust_vec_cross.length();
1080 48403797 : if (is_zero(thrust_vector_length) || is_zero(thrust_error_angle_rad)) {
1081 17904744 : thrust_vec_cross = thrust_vector_up;
1082 : } else {
1083 30499053 : thrust_vec_cross /= thrust_vector_length;
1084 : }
1085 :
1086 : // thrust_vector_correction is defined relative to the body frame but its axis `thrust_vec_cross` was computed in
1087 : // the inertial frame. First rotate it by the inverse of attitude_body to express it back in the body frame
1088 48403797 : thrust_vec_cross = attitude_body.inverse() * thrust_vec_cross;
1089 48403797 : thrust_vector_correction.from_axis_angle(thrust_vec_cross, thrust_error_angle_rad);
1090 :
1091 : // calculate the angle error in x and y.
1092 48403797 : Vector3f rotation_rad;
1093 48403797 : thrust_vector_correction.to_axis_angle(rotation_rad);
1094 48403797 : attitude_error_rad.x = rotation_rad.x;
1095 48403797 : attitude_error_rad.y = rotation_rad.y;
1096 :
1097 : // calculate the remaining rotation required after thrust vector is rotated transformed to the body frame
1098 : // heading_vector_correction
1099 48403797 : Quaternion heading_vec_correction_quat = thrust_vector_correction.inverse() * attitude_body.inverse() * attitude_target;
1100 :
1101 : // calculate the angle error in z (x and y should be zero here).
1102 48403797 : heading_vec_correction_quat.to_axis_angle(rotation_rad);
1103 48403797 : attitude_error_rad.z = rotation_rad.z;
1104 48403797 : }
1105 :
1106 : // calculates the velocity correction from an angle error. The angular velocity has acceleration and
1107 : // deceleration limits including basic jerk limiting using _input_tc
1108 106487924 : void AC_AttitudeControl::attitude_command_model(float error_angle, float desired_ang_vel, float& target_ang_vel, float& target_ang_accel, float max_ang_vel, float accel_max, float input_tc, float dt) const
1109 : {
1110 106487924 : if (!is_positive(dt)) {
1111 229345 : return;
1112 : }
1113 :
1114 : // protect against divide by zero
1115 106258579 : if (!is_positive(accel_max)) {
1116 : // no acceleration set so default to 1800 degrees/s²
1117 11 : accel_max = radians(1800);
1118 : }
1119 :
1120 106258579 : if (!is_positive(input_tc)) {
1121 : // no acceleration set so default to achieve maximum acceleration in 10 clock cycles
1122 28682806 : input_tc = dt * 10.0;
1123 : }
1124 :
1125 106258579 : shape_angle_vel_accel( error_angle, desired_ang_vel, 0.0,
1126 : 0.0, target_ang_vel, target_ang_accel,
1127 : -max_ang_vel, max_ang_vel, accel_max,
1128 : accel_max / input_tc, dt, true);
1129 106258579 : target_ang_vel += target_ang_accel * dt;
1130 : }
1131 :
1132 : // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
1133 : // This function can be used to predict the delay associated with angle requests.
1134 4579903 : void AC_AttitudeControl::command_model_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, Vector2f& target_ang_accel_rads, float dt) const
1135 : {
1136 4579903 : if (_rate_bf_ff_enabled) {
1137 : // translate the roll pitch and yaw acceleration limits to the euler axis
1138 4579903 : attitude_command_model(wrap_PI(error_angle_rad.x), 0.0, target_ang_vel_rads.x, target_ang_accel_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s);
1139 4579903 : attitude_command_model(wrap_PI(error_angle_rad.y), 0.0, target_ang_vel_rads.y, target_ang_accel_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s);
1140 : } else {
1141 0 : const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
1142 0 : const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y;
1143 0 : target_ang_vel_rads.x = angleP_roll * wrap_PI(error_angle_rad.x);
1144 0 : target_ang_vel_rads.y = angleP_pitch * wrap_PI(error_angle_rad.y);
1145 : }
1146 : // Limit the angular velocity correction
1147 4579903 : Vector3f ang_vel_rads(target_ang_vel_rads.x, target_ang_vel_rads.y, 0.0f);
1148 4579903 : ang_vel_limit(ang_vel_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), 0.0f);
1149 :
1150 4579903 : target_ang_vel_rads.x = ang_vel_rads.x;
1151 4579903 : target_ang_vel_rads.y = ang_vel_rads.y;
1152 4579903 : }
1153 :
1154 : // scale I to represent the current angle P
1155 0 : void AC_AttitudeControl::scale_I_to_angle_P()
1156 : {
1157 : Vector3f i_scale{
1158 0 : _p_angle_roll.kP() * _angle_P_scale.x,
1159 0 : _p_angle_pitch.kP() * _angle_P_scale.y,
1160 0 : _p_angle_yaw.kP() * _angle_P_scale.z
1161 0 : };
1162 0 : set_I_scale_mult(i_scale);
1163 0 : }
1164 :
1165 : // perform any required parameter conversions
1166 1285 : void AC_AttitudeControl::convert_parameters()
1167 : {
1168 : // PARAMETER_CONVERSION - Added: Jan-2026 for ArduPilot-4.7
1169 :
1170 : // return immediately if no conversion is needed
1171 1285 : if (_angle_max_deg.configured()) {
1172 29 : return;
1173 : }
1174 :
1175 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1176 : static const AP_Param::ConversionInfo conversion_info_001[] = {
1177 : { 205, 10, AP_PARAM_INT16, "Q_A_ANGLE_MAX" }, // ANGLE_MAX moved to Q_A_ANGLE_MAX
1178 : { 108, 128, AP_PARAM_FLOAT, "Q_A_RATE_WPY_MAX" }, // SLEW_YAW moved to RATE_WPY_MAX in deg/s
1179 : { 108, 256, AP_PARAM_FLOAT, "Q_A_ACC_Y_MAX" }, // ACCEL_Y_MAX moved to ACC_Y_MAX in deg/s/s
1180 : { 108, 384, AP_PARAM_FLOAT, "Q_A_ACC_R_MAX" }, // ACCEL_R_MAX moved to ACC_R_MAX in deg/s/s
1181 : { 108, 448, AP_PARAM_FLOAT, "Q_A_ACC_P_MAX" }, // ACCEL_P_MAX moved to ACC_P_MAX in deg/s/s
1182 : };
1183 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
1184 : static const AP_Param::ConversionInfo conversion_info_001[] = {
1185 : { 167, 0, AP_PARAM_INT16, "ATC_ANGLE_MAX" }, // ANGLE_MAX moved to ATC_ANGLE_MAX
1186 : { 53, 128, AP_PARAM_FLOAT, "ATC_RATE_WPY_MAX" }, // SLEW_YAW moved to RATE_WPY_MAX in deg/s
1187 : { 53, 256, AP_PARAM_FLOAT, "ATC_ACC_Y_MAX" }, // ACCEL_Y_MAX moved to ACC_Y_MAX in deg/s/s
1188 : { 53, 384, AP_PARAM_FLOAT, "ATC_ACC_R_MAX" }, // ACCEL_R_MAX moved to ACC_R_MAX in deg/s/s
1189 : { 53, 448, AP_PARAM_FLOAT, "ATC_ACC_P_MAX" }, // ACCEL_P_MAX moved to ACC_P_MAX in deg/s/s
1190 : };
1191 : #else
1192 : static const AP_Param::ConversionInfo conversion_info_001[] = {
1193 : { 34, 0, AP_PARAM_INT16, "ATC_ANGLE_MAX" }, // ANGLE_MAX moved to ATC_ANGLE_MAX
1194 : { 102, 128, AP_PARAM_FLOAT, "ATC_RATE_WPY_MAX" }, // SLEW_YAW moved to RATE_WPY_MAX in deg/s
1195 : { 102, 256, AP_PARAM_FLOAT, "ATC_ACC_Y_MAX" }, // ACCEL_Y_MAX moved to ACC_Y_MAX in deg/s/s
1196 : { 102, 384, AP_PARAM_FLOAT, "ATC_ACC_R_MAX" }, // ACCEL_R_MAX moved to ACC_R_MAX in deg/s/s
1197 : { 102, 448, AP_PARAM_FLOAT, "ATC_ACC_P_MAX" }, // ACCEL_P_MAX moved to ACC_P_MAX in deg/s/s
1198 : };
1199 : #endif
1200 1256 : AP_Param::convert_old_parameters_scaled(conversion_info_001, ARRAY_SIZE(conversion_info_001), 0.01, 0);
1201 : }
1202 :
1203 : // Limits angular velocity in roll, pitch, and yaw.
1204 : // Roll and pitch are coupled: when both limits are set, they are scaled
1205 : // proportionally as a 2D vector, preserving their ratio. Yaw is clamped independently
1206 37634590 : void AC_AttitudeControl::ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_max_rads, float ang_vel_pitch_max_rads, float ang_vel_yaw_max_rads) const
1207 : {
1208 37634590 : if (is_zero(ang_vel_roll_max_rads) || is_zero(ang_vel_pitch_max_rads)) {
1209 34625565 : if (!is_zero(ang_vel_roll_max_rads)) {
1210 0 : euler_rad.x = constrain_float(euler_rad.x, -ang_vel_roll_max_rads, ang_vel_roll_max_rads);
1211 : }
1212 34625565 : if (!is_zero(ang_vel_pitch_max_rads)) {
1213 0 : euler_rad.y = constrain_float(euler_rad.y, -ang_vel_pitch_max_rads, ang_vel_pitch_max_rads);
1214 : }
1215 : } else {
1216 3009025 : const Vector2f thrust_vector_ang_vel(euler_rad.x / ang_vel_roll_max_rads, euler_rad.y / ang_vel_pitch_max_rads);
1217 3009025 : float thrust_vector_length = thrust_vector_ang_vel.length();
1218 3009025 : if (thrust_vector_length > 1.0f) {
1219 1171 : euler_rad.x = thrust_vector_ang_vel.x * ang_vel_roll_max_rads / thrust_vector_length;
1220 1171 : euler_rad.y = thrust_vector_ang_vel.y * ang_vel_pitch_max_rads / thrust_vector_length;
1221 : }
1222 : }
1223 37634590 : if (!is_zero(ang_vel_yaw_max_rads)) {
1224 3217027 : euler_rad.z = constrain_float(euler_rad.z, -ang_vel_yaw_max_rads, ang_vel_yaw_max_rads);
1225 : }
1226 37634590 : }
1227 :
1228 : // translates body frame rotation rates and acceleration limits to the euler axis
1229 33562520 : Vector3f AC_AttitudeControl::body_to_euler_limit(const Quaternion &att, const Vector3f &body_limit)
1230 : {
1231 33562520 : if (!is_positive(body_limit.x) || !is_positive(body_limit.y) || !is_positive(body_limit.z)) {
1232 14588273 : return Vector3f { body_limit };
1233 : }
1234 :
1235 18974247 : const float phi = att.get_euler_roll();
1236 18974247 : const float theta = att.get_euler_pitch();
1237 :
1238 18974247 : const float sin_phi = constrain_float(fabsf(sinf(phi)), 0.1f, 1.0f);
1239 18974247 : const float cos_phi = constrain_float(fabsf(cosf(phi)), 0.1f, 1.0f);
1240 18974247 : const float sin_theta = constrain_float(fabsf(sinf(theta)), 0.1f, 1.0f);
1241 18974247 : const float cos_theta = constrain_float(fabsf(cosf(theta)), 0.1f, 1.0f);
1242 :
1243 18974247 : return Vector3f {
1244 18974247 : body_limit.x,
1245 18974247 : MIN(body_limit.y / cos_phi, body_limit.z / sin_phi),
1246 18974247 : MIN(MIN(body_limit.x / sin_theta, body_limit.y / (sin_phi * cos_theta)), body_limit.z / (cos_phi * cos_theta))
1247 18974247 : };
1248 : }
1249 :
1250 : // Sets attitude target to vehicle attitude and sets all rates to zero
1251 : // If reset_rate is false rates are not reset to allow the rate controllers to run
1252 16271 : void AC_AttitudeControl::reset_target_and_rate(bool reset_rate)
1253 : {
1254 : // move attitude target to current attitude
1255 16271 : _ahrs.get_quat_body_to_ned(_attitude_target);
1256 16271 : _attitude_target.to_euler(_euler_angle_target_rad);
1257 :
1258 16271 : if (reset_rate) {
1259 14963 : _ang_vel_target_rads.zero();
1260 14963 : _ang_accel_target_rads.zero();
1261 14963 : _euler_rate_target_rads.zero();
1262 : }
1263 16271 : }
1264 :
1265 : // Sets yaw target to vehicle heading and sets yaw rate to zero
1266 : // If reset_rate is false rates are not reset to allow the rate controllers to run
1267 11086303 : void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate)
1268 : {
1269 : // move attitude target to current heading
1270 11086303 : float yaw_shift = _ahrs.yaw - _euler_angle_target_rad.z;
1271 11086303 : Quaternion _attitude_target_update;
1272 11086303 : _attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
1273 11086303 : _attitude_target = _attitude_target_update * _attitude_target;
1274 :
1275 11086303 : if (reset_rate) {
1276 : // set yaw rate to zero
1277 9913702 : _euler_rate_target_rads.z = 0.0f;
1278 9913702 : _ang_accel_target_rads.z = 0.0;
1279 :
1280 : // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
1281 9913702 : euler_derivative_to_body(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads);
1282 : }
1283 11086303 : }
1284 :
1285 : // Shifts the target attitude to maintain the current error in the event of an EKF reset
1286 3590 : void AC_AttitudeControl::inertial_frame_reset()
1287 : {
1288 : // Retrieve quaternion body attitude
1289 3590 : Quaternion attitude_body;
1290 3590 : _ahrs.get_quat_body_to_ned(attitude_body);
1291 :
1292 : // Recalculate the target quaternion
1293 3590 : _attitude_target = attitude_body * _attitude_ang_error;
1294 :
1295 : // calculate the attitude target euler angles
1296 3590 : _attitude_target.to_euler(_euler_angle_target_rad);
1297 3590 : }
1298 :
1299 : // euler_derivative_to_body - transform euler angle derivative to body-frame
1300 : // Converts euler derivatives (rate, acceleration, etc.) to body-frame equivalents.
1301 : // The same transformation applies regardless of derivative order.
1302 : // Uses the kinematic relationship for 321 (yaw-pitch-roll) euler sequence.
1303 43492745 : void AC_AttitudeControl::euler_derivative_to_body(const Quaternion& att, const Vector3f& euler_derivative_rads, Vector3f& body_derivative_rads)
1304 : {
1305 43492745 : const float theta = att.get_euler_pitch();
1306 43492745 : const float phi = att.get_euler_roll();
1307 :
1308 43492745 : const float sin_theta = sinf(theta);
1309 43492745 : const float cos_theta = cosf(theta);
1310 43492745 : const float sin_phi = sinf(phi);
1311 43492745 : const float cos_phi = cosf(phi);
1312 :
1313 43492745 : body_derivative_rads.x = euler_derivative_rads.x - sin_theta * euler_derivative_rads.z;
1314 43492745 : body_derivative_rads.y = cos_phi * euler_derivative_rads.y + sin_phi * cos_theta * euler_derivative_rads.z;
1315 43492745 : body_derivative_rads.z = -sin_phi * euler_derivative_rads.y + cos_theta * cos_phi * euler_derivative_rads.z;
1316 43492745 : }
1317 :
1318 : // body_to_euler_derivative - transform body-frame derivative to euler angle derivative
1319 : // Converts body-frame derivatives (rate, acceleration, etc.) to euler equivalents.
1320 : // The same transformation applies regardless of derivative order.
1321 : // Uses the kinematic relationship for 321 (yaw-pitch-roll) euler sequence.
1322 : // Returns false if the vehicle is pitched 90 degrees up or down (gimbal lock)
1323 35566276 : bool AC_AttitudeControl::body_to_euler_derivative(const Quaternion& att, const Vector3f& body_derivative_rads, Vector3f& euler_derivative_rads)
1324 : {
1325 35566276 : const float theta = att.get_euler_pitch();
1326 35566276 : const float phi = att.get_euler_roll();
1327 :
1328 35566276 : const float sin_theta = sinf(theta);
1329 35566276 : const float cos_theta = cosf(theta);
1330 35566276 : const float sin_phi = sinf(phi);
1331 35566276 : const float cos_phi = cosf(phi);
1332 :
1333 : // When the vehicle pitches all the way up or all the way down, the euler angles become discontinuous. In this case, we just return false.
1334 35566276 : if (is_zero(cos_theta)) {
1335 382 : return false;
1336 : }
1337 :
1338 35565894 : euler_derivative_rads.x = body_derivative_rads.x + sin_phi * (sin_theta / cos_theta) * body_derivative_rads.y + cos_phi * (sin_theta / cos_theta) * body_derivative_rads.z;
1339 35565894 : euler_derivative_rads.y = cos_phi * body_derivative_rads.y - sin_phi * body_derivative_rads.z;
1340 35565894 : euler_derivative_rads.z = (sin_phi / cos_theta) * body_derivative_rads.y + (cos_phi / cos_theta) * body_derivative_rads.z;
1341 35565894 : return true;
1342 : }
1343 :
1344 : // Update rate_target_ang_vel using attitude_error_rot_vec_rad
1345 32956174 : Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad)
1346 : {
1347 32956174 : Vector3f rate_target_ang_vel;
1348 :
1349 : // Compute the roll angular velocity demand from the roll angle error
1350 32956174 : const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
1351 32956174 : if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) {
1352 32765838 : rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, angleP_roll, constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt_s);
1353 : } else {
1354 190336 : rate_target_ang_vel.x = angleP_roll * attitude_error_rot_vec_rad.x;
1355 : }
1356 :
1357 : // Compute the pitch angular velocity demand from the pitch angle error
1358 32956174 : const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y;
1359 32956174 : if (_use_sqrt_controller && !is_zero(get_accel_pitch_max_radss())) {
1360 32709253 : rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, angleP_pitch, constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt_s);
1361 : } else {
1362 246921 : rate_target_ang_vel.y = angleP_pitch * attitude_error_rot_vec_rad.y;
1363 : }
1364 :
1365 : // Compute the yaw angular velocity demand from the yaw angle error
1366 32956174 : const float angleP_yaw = _p_angle_yaw.kP() * _angle_P_scale.z;
1367 32956174 : if (_use_sqrt_controller && !is_zero(get_accel_yaw_max_radss())) {
1368 32582657 : rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, angleP_yaw, constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt_s);
1369 : } else {
1370 373517 : rate_target_ang_vel.z = angleP_yaw * attitude_error_rot_vec_rad.z;
1371 : }
1372 :
1373 32956174 : return rate_target_ang_vel;
1374 : }
1375 :
1376 : // Enable or disable body-frame feed forward
1377 0 : void AC_AttitudeControl::accel_limiting(bool enable_limits)
1378 : {
1379 0 : if (enable_limits) {
1380 : // If enabling limits, reload from eeprom or set to defaults
1381 0 : if (is_zero(_accel_roll_max_degss)) {
1382 0 : _accel_roll_max_degss.load();
1383 : }
1384 0 : if (is_zero(_accel_pitch_max_degss)) {
1385 0 : _accel_pitch_max_degss.load();
1386 : }
1387 0 : if (is_zero(_accel_yaw_max_degss)) {
1388 0 : _accel_yaw_max_degss.load();
1389 : }
1390 : } else {
1391 0 : _accel_roll_max_degss.set(0.0f);
1392 0 : _accel_pitch_max_degss.set(0.0f);
1393 0 : _accel_yaw_max_degss.set(0.0f);
1394 : }
1395 0 : }
1396 :
1397 : // Returns maximum allowable tilt angle (in centidegrees) for pilot input when in altitude hold mode.
1398 : // See get_althold_lean_angle_max_rad() for full details.
1399 843411 : float AC_AttitudeControl::get_althold_lean_angle_max_cd() const
1400 : {
1401 : // convert to centi-degrees for public interface
1402 843411 : return rad_to_cd(get_althold_lean_angle_max_rad());
1403 : }
1404 :
1405 : // Returns maximum allowable tilt angle (in radians) for pilot input when in altitude hold mode.
1406 : // Used to limit lean angle based on available thrust margin, prioritising altitude stability.
1407 31746108 : float AC_AttitudeControl::get_althold_lean_angle_max_rad() const
1408 : {
1409 31746108 : return MAX(_althold_lean_angle_max_rad, radians(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN));
1410 : }
1411 :
1412 : // Return configured tilt angle limit in centidegrees
1413 10678463 : float AC_AttitudeControl::lean_angle_max_cd() const
1414 : {
1415 10678463 : return constrain_float(_angle_max_deg.get(), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MAX) * 100;
1416 : }
1417 :
1418 : // Return configured tilt angle limit in radians
1419 55927217 : float AC_AttitudeControl::lean_angle_max_rad() const
1420 : {
1421 55927217 : return radians(constrain_float(_angle_max_deg.get(), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MAX));
1422 : }
1423 :
1424 : // Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
1425 786 : float AC_AttitudeControl::max_rate_step_bf_roll()
1426 : {
1427 786 : float dt_average = AP::scheduler().get_filtered_loop_time();
1428 786 : float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(dt_average), get_rate_roll_pid().get_filt_D_alpha(dt_average));
1429 786 : float alpha_remaining = 1 - alpha;
1430 : // todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
1431 786 : float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
1432 786 : float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_roll_pid().kD()) / _dt_s + get_rate_roll_pid().kP());
1433 786 : if (is_positive(_ang_vel_roll_max_degs)) {
1434 160 : rate_max = MIN(rate_max, get_ang_vel_roll_max_rads());
1435 : }
1436 786 : return rate_max;
1437 : }
1438 :
1439 : // Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
1440 772 : float AC_AttitudeControl::max_rate_step_bf_pitch()
1441 : {
1442 772 : const float dt_average = AP::scheduler().get_filtered_loop_time();
1443 772 : const float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(dt_average), get_rate_pitch_pid().get_filt_D_alpha(dt_average));
1444 772 : const float alpha_remaining = 1 - alpha;
1445 : // todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
1446 772 : const float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
1447 772 : float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid().kD()) / _dt_s + get_rate_pitch_pid().kP());
1448 772 : if (is_positive(_ang_vel_pitch_max_degs)) {
1449 156 : rate_max = MIN(rate_max, get_ang_vel_pitch_max_rads());
1450 : }
1451 772 : return rate_max;
1452 : }
1453 :
1454 : // Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
1455 2806 : float AC_AttitudeControl::max_rate_step_bf_yaw()
1456 : {
1457 2806 : const float dt_average = AP::scheduler().get_filtered_loop_time();
1458 2806 : const float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(dt_average), get_rate_yaw_pid().get_filt_D_alpha(dt_average));
1459 2806 : const float alpha_remaining = 1 - alpha;
1460 : // todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
1461 2806 : const float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
1462 2806 : float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid().kD()) / _dt_s + get_rate_yaw_pid().kP());
1463 2806 : if (is_positive(_ang_vel_yaw_max_degs)) {
1464 212 : rate_max = MIN(rate_max, get_ang_vel_yaw_max_rads());
1465 : }
1466 2806 : return rate_max;
1467 : }
1468 :
1469 99876 : bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
1470 : char *failure_msg,
1471 : const uint8_t failure_msg_len)
1472 : {
1473 : // validate AC_P members:
1474 : const struct {
1475 : const char *pid_name;
1476 : AC_P &p;
1477 99876 : } ps[] = {
1478 99876 : { "ANG_PIT", get_angle_pitch_p() },
1479 99876 : { "ANG_RLL", get_angle_roll_p() },
1480 99876 : { "ANG_YAW", get_angle_yaw_p() }
1481 99876 : };
1482 399504 : for (uint8_t i=0; i<ARRAY_SIZE(ps); i++) {
1483 : // all AC_P's must have a positive P value:
1484 299628 : if (!is_positive(ps[i].p.kP())) {
1485 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, ps[i].pid_name);
1486 0 : return false;
1487 : }
1488 : }
1489 :
1490 : // validate AC_PID members:
1491 : const struct {
1492 : const char *pid_name;
1493 : AC_PID &pid;
1494 99876 : } pids[] = {
1495 99876 : { "RAT_RLL", get_rate_roll_pid() },
1496 99876 : { "RAT_PIT", get_rate_pitch_pid() },
1497 99876 : { "RAT_YAW", get_rate_yaw_pid() },
1498 99876 : };
1499 399504 : for (uint8_t i=0; i<ARRAY_SIZE(pids); i++) {
1500 : // if the PID has a positive FF then we just ensure kP and
1501 : // kI aren't negative
1502 299628 : AC_PID &pid = pids[i].pid;
1503 299628 : const char *pid_name = pids[i].pid_name;
1504 299628 : if (is_positive(pid.ff())) {
1505 : // kP and kI must be non-negative:
1506 4702 : if (is_negative(pid.kP())) {
1507 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be >= 0", param_prefix, pid_name);
1508 0 : return false;
1509 : }
1510 4702 : if (is_negative(pid.kI())) {
1511 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_I must be >= 0", param_prefix, pid_name);
1512 0 : return false;
1513 : }
1514 : } else {
1515 : // kP and kI must be positive:
1516 294926 : if (!is_positive(pid.kP())) {
1517 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_P must be > 0", param_prefix, pid_name);
1518 0 : return false;
1519 : }
1520 294926 : if (!is_positive(pid.kI())) {
1521 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_I must be > 0", param_prefix, pid_name);
1522 0 : return false;
1523 : }
1524 : }
1525 : // never allow a negative D term (but zero is allowed)
1526 299628 : if (is_negative(pid.kD())) {
1527 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_%s_D must be >= 0", param_prefix, pid_name);
1528 0 : return false;
1529 : }
1530 : }
1531 :
1532 : // validate ANGLE_MAX
1533 99876 : if (_angle_max_deg.get() < 10 || _angle_max_deg.get() > 80) {
1534 0 : hal.util->snprintf(failure_msg, failure_msg_len, "%s_ANGLE_MAX must be >= 10 and <= 80", param_prefix);
1535 0 : return false;
1536 : }
1537 :
1538 99876 : return true;
1539 : }
1540 :
1541 : /*
1542 : get the slew rate for roll, pitch and yaw, for oscillation
1543 : detection in lua scripts
1544 : */
1545 2928 : void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate)
1546 : {
1547 2928 : roll_srate = get_rate_roll_pid().get_pid_info().slew_rate;
1548 2928 : pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate;
1549 2928 : yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate;
1550 2928 : }
|
