Line data Source code
1 : #pragma once
2 :
3 : /// @file AC_AttitudeControl.h
4 : /// @brief ArduCopter attitude control library
5 :
6 : #include <AP_Common/AP_Common.h>
7 : #include <AP_Param/AP_Param.h>
8 : #include <AP_Math/AP_Math.h>
9 : #include <AP_AHRS/AP_AHRS_View.h>
10 : #include <AP_Motors/AP_Motors.h>
11 : #include <AC_PID/AC_PID.h>
12 : #include <AC_PID/AC_P.h>
13 : #include <AP_BoardConfig/AP_BoardConfig.h>
14 :
15 : #define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
16 :
17 : #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
18 : #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
19 : #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
20 : #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(120.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
21 : #define AC_ATTITUDE_CONTROL_RATE_WPY_MAX_DEFAULT 60.0f // default yaw slew rate in deg/s
22 : #define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_DEGSS 1100.0f // default maximum acceleration for roll/pitch axis in deg/s/s
23 : #define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_DEGSS 270.0f // default maximum acceleration for yaw axis in deg/s/s
24 :
25 : #define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis)
26 : #define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis)
27 : #define AC_ATTITUDE_RATE_RELAX_TC 0.16f // This is used to decay the rate I term to 5% in half a second.
28 :
29 : #define AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD radians(30.0f) // Thrust angle error above which yaw corrections are limited
30 : #define AC_ATTITUDE_YAW_MAX_ERROR_ANGLE_RAD radians(45.0f) // Thrust angle error above which yaw corrections are limited
31 :
32 : #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
33 :
34 : #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
35 : #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
36 :
37 : #define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default
38 : #define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.1f // manual throttle mix default
39 : #define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
40 : #define AC_ATTITUDE_CONTROL_MIN_LIMIT 0.5f // min throttle mix upper limit
41 : #define AC_ATTITUDE_CONTROL_MAN_LIMIT 4.0f // man throttle mix upper limit
42 : #define AC_ATTITUDE_CONTROL_MAX 5.0f // maximum throttle mix default
43 :
44 : #define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
45 : #define AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH 1.0f // default angle-p/pd throttle boost threshold
46 :
47 : class AC_AttitudeControl {
48 : public:
49 1285 : AC_AttitudeControl(AP_AHRS_View &ahrs,
50 1285 : AP_Motors& motors) :
51 1285 : _p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P),
52 1285 : _p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P),
53 1285 : _p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
54 1285 : _angle_boost(0),
55 1285 : _use_sqrt_controller(true),
56 1285 : _throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
57 1285 : _throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
58 1285 : _ahrs(ahrs),
59 2570 : _motors(motors)
60 : {
61 1285 : _singleton = this;
62 1285 : AP_Param::setup_object_defaults(this, var_info);
63 1285 : }
64 :
65 29402 : static AC_AttitudeControl *get_singleton(void) {
66 29402 : return _singleton;
67 : }
68 :
69 : // Empty destructor to suppress compiler warning
70 14 : virtual ~AC_AttitudeControl() {}
71 :
72 : // Sets the internal controller timestep (in seconds).
73 : // This is the elapsed time since the last controller update and is used in rate and smoothing calculations.
74 35255076 : void set_dt_s(float dt_s) { _dt_s = dt_s; }
75 :
76 : // Returns the internal controller timestep (in seconds).
77 4579903 : float get_dt_s() const { return _dt_s; }
78 :
79 : // pid accessors
80 408932 : AC_P& get_angle_roll_p() { return _p_angle_roll; }
81 408924 : AC_P& get_angle_pitch_p() { return _p_angle_pitch; }
82 103640 : AC_P& get_angle_yaw_p() { return _p_angle_yaw; }
83 : virtual AC_PID& get_rate_roll_pid() = 0;
84 : virtual AC_PID& get_rate_pitch_pid() = 0;
85 : virtual AC_PID& get_rate_yaw_pid() = 0;
86 : virtual const AC_PID& get_rate_roll_pid() const = 0;
87 : virtual const AC_PID& get_rate_pitch_pid() const = 0;
88 : virtual const AC_PID& get_rate_yaw_pid() const = 0;
89 :
90 : // get the roll acceleration limit in radians/s/s
91 104140968 : float get_accel_roll_max_radss() const { return radians(_accel_roll_max_degss); }
92 :
93 : // Sets the roll acceleration limit in radians/s/s
94 131 : void set_accel_roll_max_radss(float accel_roll_max_radss) { _accel_roll_max_degss.set(degrees(accel_roll_max_radss)); }
95 :
96 : // Sets and saves the roll acceleration limit in radians/s/s
97 3 : void save_accel_roll_max_radss(float accel_roll_max_radss) { _accel_roll_max_degss.set_and_save(degrees(accel_roll_max_radss)); }
98 :
99 : // get the pitch acceleration limit in radians/s/s
100 104084383 : float get_accel_pitch_max_radss() const { return radians(_accel_pitch_max_degss); }
101 :
102 : // Sets the pitch acceleration limit in radians/s/s
103 130 : void set_accel_pitch_max_radss(float accel_pitch_max_radss) { _accel_pitch_max_degss.set(degrees(accel_pitch_max_radss)); }
104 :
105 : // Sets and saves the pitch acceleration limit in radians/s/s
106 3 : void save_accel_pitch_max_radss(float accel_pitch_max_radss) { _accel_pitch_max_degss.set_and_save(degrees(accel_pitch_max_radss)); }
107 :
108 : // get the yaw acceleration limit in radians/s/s
109 130850216 : float get_accel_yaw_max_radss() const { return radians(_accel_yaw_max_degss); }
110 :
111 : // Sets the yaw acceleration limit in radians/s/s
112 137 : void set_accel_yaw_max_radss(float accel_yaw_max_radss) { _accel_yaw_max_degss.set(degrees(accel_yaw_max_radss)); }
113 :
114 : // Sets and saves the yaw acceleration limit in radians/s/s
115 3 : void save_accel_yaw_max_radss(float accel_yaw_max_radss) { _accel_yaw_max_degss.set_and_save(degrees(accel_yaw_max_radss)); }
116 :
117 : // get the roll angular velocity limit in radians/s
118 1492408 : float get_ang_vel_roll_max_rads() const { return radians(_ang_vel_roll_max_degs); }
119 :
120 : // set the roll angular velocity limit in radians/s
121 119 : void set_ang_vel_roll_max_rads(float vel_roll_max_rads) { _ang_vel_roll_max_degs.set(degrees(vel_roll_max_rads)); }
122 :
123 : // get the pitch angular velocity limit in radians/s
124 1492404 : float get_ang_vel_pitch_max_rads() const { return radians(_ang_vel_pitch_max_degs); }
125 :
126 : // set the pitch angular velocity limit in radians/s
127 118 : void set_ang_vel_pitch_max_rads(float vel_pitch_max_rads) { _ang_vel_pitch_max_degs.set(degrees(vel_pitch_max_rads)); }
128 :
129 : // get the yaw angular velocity limit in radians/s
130 219 : float get_ang_vel_yaw_max_rads() const { return radians(_ang_vel_yaw_max_degs); }
131 :
132 : // set the yaw angular velocity limit in radians/s
133 123 : void set_ang_vel_yaw_max_rads(float vel_yaw_max_rads) { _ang_vel_yaw_max_degs.set(degrees(vel_yaw_max_rads)); }
134 :
135 : // get the slew yaw rate limit in radians/s
136 : float get_slew_yaw_max_rads() const;
137 :
138 : // get the rate control input smoothing time constant
139 15182 : float get_input_tc() const { return _input_tc; }
140 :
141 : // set the rate control input smoothing time constant
142 0 : void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); }
143 :
144 : // rate loop visible functions
145 : // Ensure attitude controller have zero errors to relax rate controller output
146 : void relax_attitude_controllers();
147 :
148 : // Used by child class AC_AttitudeControl_TS to change behaviour for tailsitter quadplanes
149 0 : virtual void relax_attitude_controllers(bool exclude_pitch) { relax_attitude_controllers(); }
150 :
151 : // reset rate controller I terms
152 : void reset_rate_controller_I_terms();
153 :
154 : // Smoothly decays I-term to zero over ~0.5s. Useful for transitions to prevent overshoot.
155 : void reset_rate_controller_I_terms_smoothly();
156 :
157 : // Reduce attitude control gains while landed to stop ground resonance
158 : void landed_gain_reduction(bool landed);
159 :
160 : // Sets the attitude and angular rate targets to the current vehicle state.
161 : // If reset_rate is false, only the attitude target is reset and current rate targets are preserved.
162 : void reset_target_and_rate(bool reset_rate = true);
163 :
164 : // Sets the yaw angle target to the vehicle's current heading. Optionally resets yaw rate to zero.
165 : void reset_yaw_target_and_rate(bool reset_rate = true);
166 :
167 : // handle reset of attitude from EKF since the last iteration
168 : void inertial_frame_reset();
169 :
170 : // Commands a body-frame roll angle (in centidegrees), an euler pitch angle (in centidegrees), and a yaw rate (in centidegrees/s).
171 : // See input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad() for full details.
172 : // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes)
173 0 : virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_cd(bool plane_controls, float euler_roll_angle_cd,
174 0 : float euler_pitch_angle_cd, float euler_yaw_rate_cds) {}
175 :
176 : // Commands a body-frame roll angle (in radians), an euler pitch angle (in radians), and a yaw rate (in radians/s).
177 : // Used by tailsitter quadplanes. Optionally swaps roll and yaw effects as pitch nears 90° if plane_controls is true.
178 : // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes)
179 0 : virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(bool plane_controls, float euler_roll_angle_rad,
180 0 : float euler_pitch_angle_rad, float euler_yaw_rate_rads) {}
181 :
182 : ////// begin rate update functions //////
183 : // These functions all update _ang_vel_body_rads which is used as the rate target by the rate controller.
184 : // Since _ang_vel_body_rads can be seen by the rate controller thread all these functions only set it
185 : // at the end once all of the calculations have been performed. This avoids intermediate results being
186 : // used by the rate controller when running concurrently. _ang_vel_body_rads is accessed so commonly that
187 : // locking proves to be moderately expensive, however since this is changing incrementally values combining
188 : // previous and current elements are safe and do not have an impact on control.
189 : // Any additional functions that are added to manipulate _ang_vel_body_rads should follow this pattern.
190 :
191 : // Calculates the body frame angular velocities to follow the target attitude
192 : // This is used by most of the subsequent functions
193 : void attitude_controller_run_quat();
194 :
195 : // Sets an attitude target using a quaternion and a body-frame angular velocity input (rad/s).
196 : // The desired quaternion is incrementally advanced each timestep using the (limited) angular velocity input.
197 : // If body-frame rate feedforward shaping is enabled, rate/accel targets are generated with acceleration limits
198 : // and input time constants; otherwise the targets are set directly.
199 : virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body_rads);
200 :
201 : // Sets the desired roll and pitch angles (in centidegrees) and yaw rate (in centidegrees/s).
202 : // See input_euler_angle_roll_pitch_euler_rate_yaw_rad() for full details.
203 : void input_euler_angle_roll_pitch_euler_rate_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
204 :
205 : // Sets the desired roll and pitch angle inputs (radians) and a yaw rate input (radians/s).
206 : // Used when roll/pitch stabilization is required while yaw is controlled as a rate.
207 : // If body-frame rate feedforward shaping is enabled, shapes Euler rate/acceleration targets
208 : // with configured limits and time constants and converts them back to body-frame targets.
209 : // Otherwise, updates the attitude target directly and zeros rate/accel feedforward targets.
210 : virtual void input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads);
211 :
212 : // Sets the desired roll, pitch, and yaw angles (in centidegrees).
213 : // See input_euler_angle_roll_pitch_yaw_rad() for full details.
214 : void 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);
215 :
216 : // Sets the desired roll, pitch, and yaw angle inputs (radians) to follow an absolute attitude setpoint.
217 : // Optional yaw slew limiting constrains the rate of change of the yaw target.
218 : // If body-frame rate feedforward shaping is enabled, shapes Euler rate/acceleration targets
219 : // with configured limits and time constants and converts them back to body-frame targets.
220 : // Otherwise, updates the attitude target directly (with optional yaw slew) and zeros rate/accel feedforward targets.
221 : virtual void 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);
222 :
223 : // Sets the desired roll, pitch, and yaw Euler angle rate inputs (radians/s).
224 : // If body-frame rate feedforward shaping is enabled, the inputs are shaped using acceleration limits
225 : // and time constants to generate Euler rate/acceleration targets, which are then converted into
226 : // body-frame angular velocity/acceleration targets for the rate controller.
227 : // Otherwise, Euler angle targets are integrated directly from the rate inputs and feedforward targets are zeroed.
228 : virtual void input_euler_rate_roll_pitch_yaw_rads(float euler_roll_rate_rads, float euler_pitch_rate_rads, float euler_yaw_rate_rads);
229 :
230 : // Sets the desired roll, pitch, and yaw angular rates (in centidegrees/s).
231 : // See input_rate_bf_roll_pitch_yaw_rads() for full details.
232 : void input_rate_bf_roll_pitch_yaw_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
233 :
234 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
235 : // Used by fully stabilized acro modes.
236 : // If body-frame rate feedforward shaping is enabled, the inputs are shaped using acceleration limits
237 : // and time constants to generate body-frame angular velocity/acceleration targets for the rate controller.
238 : // Otherwise, the attitude target is incrementally rotated using the rate inputs and feedforward targets are zeroed.
239 : virtual void input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads);
240 :
241 : // Sets the desired roll, pitch, and yaw angular rates in body-frame (in centidegrees/s).
242 : // See input_rate_bf_roll_pitch_yaw_2_rads() for full details.
243 : void input_rate_bf_roll_pitch_yaw_2_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
244 :
245 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
246 : // Used by Copter's rate-only acro mode.
247 : // Shapes the body-frame rate inputs using acceleration limits and time constants to produce
248 : // body-frame angular velocity/acceleration targets for the rate controller.
249 : // Attitude targets are updated from the current AHRS attitude to keep target state coherent for mode transitions.
250 : virtual void input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads);
251 :
252 : // Sets the desired roll, pitch, and yaw angular rates in body-frame (in centidegrees/s).
253 : // See input_rate_bf_roll_pitch_yaw_3_rads() for full details.
254 : void input_rate_bf_roll_pitch_yaw_3_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
255 :
256 : // Sets the desired roll, pitch, and yaw body-frame angular rate inputs (radians/s).
257 : // Used by Plane's acro mode with rate error integration.
258 : // Maintains an integrated attitude error quaternion using (target_rate - gyro) and combines it
259 : // with shaped rate inputs to form the final body-frame angular velocity target for the rate controller.
260 : virtual void input_rate_bf_roll_pitch_yaw_3_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads);
261 :
262 : // Directly sets the body-frame angular rates without smoothing (in centidegrees/s).
263 : // See input_rate_bf_roll_pitch_yaw_no_shaping_rads() for full details.
264 : void input_rate_bf_roll_pitch_yaw_no_shaping_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
265 :
266 : // Directly sets body-frame angular rate targets (radians/s) without shaping.
267 : // Used when an external controller (e.g. fixed-wing controller) provides VTOL body rates.
268 : // No smoothing, acceleration limiting, or input shaping is applied.
269 : void input_rate_bf_roll_pitch_yaw_no_shaping_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads);
270 :
271 : // Applies a one-time angular offset in body-frame roll/pitch/yaw angles (in radians).
272 : // Used for initiating step responses during autotuning or manual test inputs.
273 : virtual void 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);
274 :
275 : // Applies a one-time body-frame angular rate step (radians/s) in roll, pitch, and yaw.
276 : // Used to inject discrete disturbances or step inputs for system identification.
277 : // This sets the body-frame rate command directly for this update; no shaping is applied.
278 : virtual void 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);
279 :
280 : // Sets the desired thrust vector and a yaw/heading rate input (radians/s).
281 : // Used for tilt-based navigation with independent yaw control.
282 : // The thrust vector determines the desired tilt (attitude) while the heading rate commands yaw.
283 : // Optional yaw slew limiting constrains the heading rate input.
284 : virtual void input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw = true);
285 :
286 : // Sets the desired thrust vector, heading angle (radians), and heading rate input (radians/s).
287 : // Used when thrust direction (tilt) is commanded independently from yaw/heading.
288 : // Heading rate is constrained using the configured yaw slew rate limit (0 disables limiting).
289 : virtual void input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads);
290 :
291 : // Sets the desired thrust vector and heading (in centidegrees), with zero heading rate.
292 : // See input_thrust_vector_heading_rad() for full details.
293 : void input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_rad) {input_thrust_vector_heading_rad(thrust_vector, heading_rad, 0.0f);}
294 :
295 : ////// end rate update functions //////
296 :
297 : // Converts a thrust vector and heading angle to a body-to-NED quaternion attitude
298 : Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle_rad) const;
299 :
300 : // Run angular velocity controller and send outputs to the motors
301 : virtual void rate_controller_run() = 0;
302 :
303 : // reset the rate controller target loop updates
304 : void rate_controller_target_reset();
305 :
306 : // Run the angular velocity controller with a specified timestep. Must be implemented by derived class.
307 0 : virtual void rate_controller_run_dt(const Vector3f& gyro_rads, float dt) { AP_BoardConfig::config_error("rate_controller_run_dt() must be defined"); };
308 :
309 : // euler_derivative_to_body - transform euler angle derivative to body-frame
310 : // Converts euler derivatives (rate, acceleration, etc.) to body-frame equivalents.
311 : // The same transformation applies regardless of derivative order.
312 : // Uses the kinematic relationship for 321 (yaw-pitch-roll) euler sequence.
313 : void euler_derivative_to_body(const Quaternion& att, const Vector3f& euler_derivative_rads, Vector3f& body_derivative_rads);
314 :
315 : // body_to_euler_derivative - transform body-frame derivative to euler angle derivative
316 : // Converts body-frame derivatives (rate, acceleration, etc.) to euler equivalents.
317 : // The same transformation applies regardless of derivative order.
318 : // Uses the kinematic relationship for 321 (yaw-pitch-roll) euler sequence.
319 : // Returns false if the vehicle is pitched 90 degrees up or down (gimbal lock)
320 : bool body_to_euler_derivative(const Quaternion& att, const Vector3f& body_derivative_rads, Vector3f& euler_derivative_rads);
321 :
322 : // Specifies whether the attitude controller should use the square root controller in the attitude correction.
323 : // This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
324 4698 : void use_sqrt_controller(bool use_sqrt_cont) { _use_sqrt_controller = use_sqrt_cont; }
325 :
326 : // Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
327 : // attitude controller's target attitude.
328 : // **NOTE** Using vector3f*deg(100) is more efficient than deg(vector3f)*100 or deg(vector3d*100) because it gives the
329 : // same result with the fewest multiplications. Even though it may look like a bug, it is intentional. See issue 4895.
330 624141 : Vector3f get_att_target_euler_cd() const { return _euler_angle_target_rad * degrees(100.0f); }
331 65417834 : const Vector3f & get_att_target_euler_rad() const { return _euler_angle_target_rad; }
332 :
333 : // Returns the target attitude quaternion used by the attitude controller.
334 : // This represents the desired orientation in NED earth frame.
335 223475 : Quaternion get_attitude_target_quat() const { return _attitude_target; }
336 :
337 : // Return the angular velocity of the target (setpoint) [rad/s] in the target attitude frame
338 4516220 : const Vector3f& get_attitude_target_ang_vel() const { return _ang_vel_target_rads;}
339 :
340 : // Return the angle between the target thrust vector and the current thrust vector.
341 22485066 : float get_att_error_angle_deg() const { return degrees(_thrust_error_angle_rad); }
342 :
343 : // Returns the current attitude target as 321 Euler angles in centidegrees.
344 : // Note: Centidegrees are used for legacy compatibility with older messaging formats.
345 374475 : void rate_bf_yaw_target(float rate_cds) { _ang_vel_body_rads.z = cd_to_rad(rate_cds); }
346 :
347 : // Set x-axis system identification angular velocity in radians/s
348 0 : void rate_bf_roll_sysid_rads(float rate_rads) { _sysid_ang_vel_body_rads.x = rate_rads; }
349 :
350 : // Set y-axis system identification angular velocity in radians/s
351 0 : void rate_bf_pitch_sysid_rads(float rate_rads) { _sysid_ang_vel_body_rads.y = rate_rads; }
352 :
353 : // Set z-axis system identification angular velocity in radians/s
354 0 : void rate_bf_yaw_sysid_rads(float rate_rads) { _sysid_ang_vel_body_rads.z = rate_rads; }
355 :
356 : // Set x-axis system identification actuator
357 0 : void actuator_roll_sysid(float command) { _actuator_sysid.x = command; }
358 :
359 : // Set y-axis system identification actuator
360 0 : void actuator_pitch_sysid(float command) { _actuator_sysid.y = command; }
361 :
362 : // Set z-axis system identification actuator
363 0 : void actuator_yaw_sysid(float command) { _actuator_sysid.z = command; }
364 :
365 : // Return roll rate step size in radians/s that results in maximum output after 4 time steps
366 : float max_rate_step_bf_roll();
367 :
368 : // Return pitch rate step size in radians/s that results in maximum output after 4 time steps
369 : float max_rate_step_bf_pitch();
370 :
371 : // Return yaw rate step size in radians/s that results in maximum output after 4 time steps
372 : float max_rate_step_bf_yaw();
373 :
374 : // Return roll step size in radians that results in maximum output after 4 time steps
375 393 : float max_angle_step_bf_roll() { return max_rate_step_bf_roll() / _p_angle_roll.kP(); }
376 :
377 : // Return pitch step size in radians that results in maximum output after 4 time steps
378 386 : float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch() / _p_angle_pitch.kP(); }
379 :
380 : // Return yaw step size in radians that results in maximum output after 4 time steps
381 1403 : float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw() / _p_angle_yaw.kP(); }
382 :
383 : // Return the body-frame angular velocity (in rad/s) used by the angular velocity controller.
384 5193374 : Vector3f rate_bf_targets() const { return _ang_vel_body_rads + _sysid_ang_vel_body_rads; }
385 :
386 : // return the angular velocity of the target (setpoint) attitude rad/s
387 928202 : const Vector3f& get_rate_ef_target_rads() const { return _euler_rate_target_rads; }
388 :
389 : // Enable or disable body-frame feed forward
390 2523 : void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled.set(enable_or_disable); }
391 :
392 : // Enable or disable body-frame feed forward and save
393 0 : void bf_feedforward_save(bool enable_or_disable) { _rate_bf_ff_enabled.set_and_save(enable_or_disable); }
394 :
395 : // Return body-frame feed forward setting
396 2772565 : bool get_bf_feedforward() { return _rate_bf_ff_enabled; }
397 :
398 : // Enable or disable body-frame feed forward
399 : void accel_limiting(bool enable_or_disable);
400 :
401 : // Update Alt_Hold angle maximum
402 : virtual void update_althold_lean_angle_max(float throttle_in) = 0;
403 :
404 : // Set output throttle
405 : virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
406 :
407 : // get throttle passed into attitude controller (i.e. throttle_in provided to set_throttle_out)
408 26127671 : float get_throttle_in() const { return _throttle_in; }
409 :
410 : // Return throttle increase applied for tilt compensation
411 986292 : float angle_boost() const { return _angle_boost; }
412 :
413 : // Returns maximum allowable tilt angle (in radians) for pilot input when in altitude hold mode.
414 : // Used to limit lean angle based on available thrust margin, prioritising altitude stability.
415 : virtual float get_althold_lean_angle_max_rad() const;
416 :
417 : // Returns maximum allowable tilt angle (in centidegrees) for pilot input when in altitude hold mode.
418 : // See get_althold_lean_angle_max_rad() for full details.
419 : float get_althold_lean_angle_max_cd() const;
420 :
421 : // Return configured tilt angle limit in centidegrees
422 : float lean_angle_max_cd() const;
423 :
424 : // Return configured tilt angle limit in radians
425 : float lean_angle_max_rad() const;
426 :
427 : // Return tilt angle in degrees
428 855370 : float lean_angle_deg() const { return degrees(_thrust_angle_rad); }
429 0 : float lean_angle_rad() const { return _thrust_angle_rad; }
430 :
431 : // Calculates the velocity correction from an angle error, applying acceleration/deceleration limits and a simple jerk-limiting mechanism via the smoothing gain.
432 : void 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;
433 :
434 : // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
435 : // This function can be used to predict the delay associated with angle requests.
436 : void command_model_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, Vector2f& target_ang_accel_rads, float dt) const;
437 :
438 : // Limits angular velocity in roll, pitch, and yaw.
439 : // Roll and pitch are coupled: when both limits are set, they are scaled
440 : // proportionally as a 2D vector, preserving their ratio. Yaw is clamped independently
441 : void 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;
442 :
443 : // translates body frame rotation rates and acceleration limits to the euler axis
444 : Vector3f body_to_euler_limit(const Quaternion &att, const Vector3f &body_limit);
445 :
446 : // Calculates the body frame angular velocities to follow the target attitude
447 : void update_attitude_target();
448 :
449 : // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
450 : // The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
451 : void 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;
452 :
453 : // thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
454 : // The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
455 : void 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;
456 :
457 : // sanity check parameters. should be called once before take-off
458 83 : virtual void parameter_sanity_check() {}
459 :
460 : // Optional override: sets the notch filter sample rate for rate PID controllers (if supported).
461 : // Default implementation is empty.
462 0 : virtual void set_notch_sample_rate(float sample_rate) {}
463 :
464 : // Returns true if throttle RPY mix is at its minimum value.
465 : // Used to determine if attitude control should be reduced in favor of vertical thrust.
466 0 : virtual bool is_throttle_mix_min() const { return true; }
467 :
468 : // control rpy throttle mix
469 0 : virtual void set_throttle_mix_min() {}
470 0 : virtual void set_throttle_mix_man() {}
471 0 : virtual void set_throttle_mix_max(float ratio) {}
472 0 : virtual void set_throttle_mix_value(float value) {}
473 0 : virtual float get_throttle_mix(void) const { return 0; }
474 :
475 : // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage on heli
476 0 : virtual void use_leaky_i(bool leaky_i) {}
477 :
478 : // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
479 0 : virtual void set_hover_roll_trim_scalar(float scalar) {}
480 :
481 : // Return angle in centidegrees to be added to roll angle for hover collective learn. Used by heli to counteract
482 : // tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
483 16641328 : virtual float get_roll_trim_cd() { return 0;}
484 :
485 : // Return angle in radians to be added to roll angle. Used by heli to counteract
486 : // tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
487 19261155 : float get_roll_trim_rad() { return cd_to_rad(get_roll_trim_cd()); }
488 :
489 : // provide feedback on whether arming would be a good idea right now:
490 : bool pre_arm_checks(const char *param_prefix,
491 : char *failure_msg,
492 : const uint8_t failure_msg_len);
493 :
494 : // enable inverted flight on backends that support it
495 0 : virtual void set_inverted_flight(bool inverted) {}
496 :
497 : // enable accessor for inverted flight flag on backends that support it
498 0 : virtual bool get_inverted_flight() { return false;}
499 :
500 : // get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
501 : void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
502 :
503 : // Sets the roll and pitch rate shaping time constant
504 1267 : void set_roll_pitch_rate_tc(float input_tc) { _rate_rp_tc = input_tc; }
505 :
506 : // Sets the yaw rate shaping time constant
507 2597625 : void set_yaw_rate_tc(float input_tc) { _rate_y_tc = input_tc; }
508 :
509 : // setup a one loop angle P scale multiplier. This replaces any previous scale applied
510 : // so should only be used when only one source of scaling is needed
511 201445 : void set_angle_P_scale(const Vector3f &angle_P_scale) { _angle_P_scale = angle_P_scale; }
512 :
513 : // setup a one loop angle P scale multiplier, multiplying by any
514 : // previously applied scale from this loop. This allows for more
515 : // than one type of scale factor to be applied for different
516 : // purposes
517 30664814 : void set_angle_P_scale_mult(const Vector3f &angle_P_scale) { _angle_P_scale *= angle_P_scale; }
518 :
519 : // get the value of the angle P scale that was used in the last loop
520 4724142 : const Vector3f &get_last_angle_P_scale(void) const { return _angle_P_scale_used; }
521 :
522 : // setup a one loop PD scale multiplier, multiplying by any
523 : // previously applied scale from this loop. This allows for more
524 : // than one type of scale factor to be applied for different
525 : // purposes
526 30664814 : void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; }
527 :
528 : // setup a one loop I scale multiplier, multiplying by any
529 : // previously applied scale from this loop. This allows for more
530 : // than one type of scale factor to be applied for different
531 : // purposes
532 0 : void set_I_scale_mult(const Vector3f &i_scale) { _i_scale *= i_scale; }
533 :
534 : // scale I to represent the control given by angle P
535 : void scale_I_to_angle_P();
536 :
537 : // write RATE message
538 : void Write_Rate(const AC_PosControl &pos_control) const;
539 :
540 : // write ANG message
541 : void Write_ANG() const;
542 :
543 : // perform any required parameter conversions
544 : void convert_parameters();
545 :
546 : // User settable parameters
547 : static const struct AP_Param::GroupInfo var_info[];
548 :
549 : static constexpr Vector3f VECTORF_111{1.0f,1.0f,1.0f};
550 :
551 : protected:
552 :
553 : // Update rate_target_ang_vel using attitude_error_rot_vec_rad
554 : Vector3f update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad);
555 :
556 : // Returns the most recent angular velocity reading (rad/s) for the rate controller.
557 : // Ensures minimum latency when rate control is run before or after attitude control.
558 : const Vector3f get_latest_gyro() const;
559 :
560 : // Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
561 : AP_Float _rate_wp_yaw_max_degs;
562 :
563 : // Maximum angular velocity (in degrees/second) for earth-frame roll, pitch and yaw axis
564 : AP_Float _ang_vel_roll_max_degs;
565 : AP_Float _ang_vel_pitch_max_degs;
566 : AP_Float _ang_vel_yaw_max_degs;
567 :
568 : // Maximum rotation acceleration for earth-frame roll axis
569 : AP_Float _accel_roll_max_degss;
570 :
571 : // Maximum rotation acceleration for earth-frame pitch axis
572 : AP_Float _accel_pitch_max_degss;
573 :
574 : // Maximum rotation acceleration for earth-frame yaw axis
575 : AP_Float _accel_yaw_max_degss;
576 :
577 : // Enable/Disable body frame rate feed forward
578 : AP_Int8 _rate_bf_ff_enabled;
579 :
580 : // Enable/Disable angle boost
581 : AP_Int8 _angle_boost_enabled;
582 :
583 : // angle controller P objects
584 : AC_P _p_angle_roll;
585 : AC_P _p_angle_pitch;
586 : AC_P _p_angle_yaw;
587 :
588 : // Angle limit time constant (to maintain altitude)
589 : AP_Float _angle_limit_tc;
590 :
591 : // Time constant [s] used to smooth user/control input changes before being passed to the rate controller.
592 : // Affects angular response stiffness.
593 : AP_Float _input_tc;
594 :
595 : // Controller gain multiplyer to be used when landed
596 : AP_Float _land_roll_mult;
597 : AP_Float _land_pitch_mult;
598 : AP_Float _land_yaw_mult;
599 :
600 : // Angle limit
601 : AP_Float _angle_max_deg;
602 :
603 : // Latest body-frame gyro measurement (rad/s) used by rate controller
604 : Vector3f _rate_gyro_rads;
605 : // timestamp of the latest gyro measurement (in microseconds) value used by the rate controller
606 : uint64_t _rate_gyro_time_us;
607 :
608 : // Intersampling period in seconds
609 : float _dt_s;
610 :
611 : // This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
612 : // attitude used in the attitude controller, in radians.
613 : Vector3f _euler_angle_target_rad;
614 :
615 : // This represents the angular velocity of the target (setpoint) attitude used in
616 : // the attitude controller as 321-intrinsic euler angle derivatives, in radians per
617 : // second.
618 : Vector3f _euler_rate_target_rads;
619 :
620 : // This represents a quaternion rotation in NED frame to the target (setpoint)
621 : // attitude used in the attitude controller.
622 : Quaternion _attitude_target;
623 :
624 : // This represents the angular velocity of the target (setpoint) attitude used in
625 : // the attitude controller as an angular velocity vector, in radians per second in
626 : // the target attitude frame.
627 : Vector3f _ang_vel_target_rads;
628 : Vector3f _ang_accel_target_rads;
629 :
630 : // This represents the angular velocity in radians per second in the body frame, used in the angular
631 : // velocity controller and most importantly the rate controller.
632 : Vector3f _ang_vel_body_rads;
633 :
634 : // This is the angular velocity in radians per second in the body frame, added to the output angular
635 : // attitude controller by the System Identification Mode.
636 : // It is reset to zero immediately after it is used.
637 : Vector3f _sysid_ang_vel_body_rads;
638 :
639 : // System Identification actuator override applied to motor commands.
640 : // This is a unitless signal added post-PID for excitation purposes and is reset every loop.
641 : // It is reset to zero immediately after it is used.
642 : Vector3f _actuator_sysid;
643 :
644 : // This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
645 : Quaternion _attitude_ang_error;
646 :
647 : // The angle between the target thrust vector and the current thrust vector.
648 : float _thrust_angle_rad;
649 :
650 : // The angle between the target thrust vector and the current thrust vector.
651 : float _thrust_error_angle_rad;
652 :
653 : // throttle provided as input to attitude controller. This does not include angle boost.
654 : float _throttle_in = 0.0f;
655 :
656 : // Throttle increase applied to maintain vertical thrust during attitude tilt.
657 : // Used logging for logging and diagnostics.
658 : float _angle_boost;
659 :
660 : // Specifies whether the attitude controller should use the square root controller in the attitude correction.
661 : // This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
662 : bool _use_sqrt_controller;
663 :
664 : // Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
665 : float _althold_lean_angle_max_rad = 0.0f;
666 :
667 : // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
668 : float _throttle_rpy_mix_desired;
669 :
670 : // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
671 : float _throttle_rpy_mix;
672 :
673 : // Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
674 : float _feedforward_scalar = 1.0f;
675 :
676 : // Rate controller input smoothing time constants
677 : // Time constant for shaping roll/pitch rate input [s]
678 : float _rate_rp_tc;
679 : // Time constant for shaping yaw rate input [s]
680 : float _rate_y_tc;
681 :
682 : // Active scaling applied to Angle P gains for roll, pitch, yaw
683 : Vector3f _angle_P_scale{1,1,1};
684 :
685 : // Active scaling applied to Angle P gains this loop (for logging/debugging)
686 : Vector3f _angle_P_scale_used;
687 :
688 : // Proportional-Derivative gains applied dynamically per axis
689 : Vector3f _pd_scale{1,1,1};
690 :
691 : // Proportional-Derivative gains this loop (for logging/debugging)
692 : Vector3f _pd_scale_used;
693 :
694 : // Integrator gains applied dynamically per axis
695 : Vector3f _i_scale{1,1,1};
696 :
697 : // Integrator gains this loop (for logging/debugging)
698 : Vector3f _i_scale_used;
699 :
700 : // Ratio of normal to reduced rate controller gain when landed to suppress ground resonance
701 : float _landed_gain_ratio;
702 :
703 : // References to external libraries
704 : const AP_AHRS_View& _ahrs;
705 : AP_Motors& _motors;
706 :
707 : static AC_AttitudeControl *_singleton;
708 :
709 : public:
710 : // structure for angle and/or rate target
711 : enum class HeadingMode {
712 : Angle_Only,
713 : Angle_And_Rate,
714 : Rate_Only
715 : };
716 : struct HeadingCommand {
717 : float yaw_angle_rad;
718 : float yaw_rate_rads;
719 : HeadingMode heading_mode;
720 : };
721 : void input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading);
722 : };
|
