Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : // Initial Code by Jon Challinger
17 : // Modified by Paul Riseborough
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include "AP_PitchController.h"
21 : #include <AP_AHRS/AP_AHRS.h>
22 : #include <AP_Scheduler/AP_Scheduler.h>
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : const AP_Param::GroupInfo AP_PitchController::var_info[] = {
27 :
28 : // @Param: 2SRV_TCONST
29 : // @DisplayName: Pitch Time Constant
30 : // @Description: Time constant in seconds from demanded to achieved pitch angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
31 : // @Range: 0.4 1.0
32 : // @Units: s
33 : // @Increment: 0.1
34 : // @User: Advanced
35 : AP_GROUPINFO("2SRV_TCONST", 0, AP_PitchController, gains.tau, 0.5f),
36 :
37 : // index 1 to 3 reserved for old PID values
38 :
39 : // @Param: 2SRV_RMAX_UP
40 : // @DisplayName: Pitch up max rate
41 : // @Description: This sets the maximum nose up pitch rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables the limit.
42 : // @Range: 0 100
43 : // @Units: deg/s
44 : // @Increment: 1
45 : // @User: Advanced
46 : AP_GROUPINFO("2SRV_RMAX_UP", 4, AP_PitchController, gains.rmax_pos, 0.0f),
47 :
48 : // @Param: 2SRV_RMAX_DN
49 : // @DisplayName: Pitch down max rate
50 : // @Description: This sets the maximum nose down pitch rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables the limit.
51 : // @Range: 0 100
52 : // @Units: deg/s
53 : // @Increment: 1
54 : // @User: Advanced
55 : AP_GROUPINFO("2SRV_RMAX_DN", 5, AP_PitchController, gains.rmax_neg, 0.0f),
56 :
57 : // @Param: 2SRV_RLL
58 : // @DisplayName: Roll compensation
59 : // @Description: Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.
60 : // @Range: 0.7 1.5
61 : // @Increment: 0.05
62 : // @User: Standard
63 : AP_GROUPINFO("2SRV_RLL", 6, AP_PitchController, _roll_ff, 1.0f),
64 :
65 : // index 7, 8 reserved for old IMAX, FF
66 :
67 : // @Param: _RATE_P
68 : // @DisplayName: Pitch axis rate controller P gain
69 : // @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
70 : // @Range: 0.08 0.35
71 : // @Increment: 0.005
72 : // @User: Standard
73 :
74 : // @Param: _RATE_I
75 : // @DisplayName: Pitch axis rate controller I gain
76 : // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
77 : // @Range: 0.01 0.6
78 : // @Increment: 0.01
79 : // @User: Standard
80 :
81 : // @Param: _RATE_IMAX
82 : // @DisplayName: Pitch axis rate controller I gain maximum
83 : // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
84 : // @Range: 0 1
85 : // @Increment: 0.01
86 : // @User: Standard
87 :
88 : // @Param: _RATE_D
89 : // @DisplayName: Pitch axis rate controller D gain
90 : // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
91 : // @Range: 0.001 0.03
92 : // @Increment: 0.001
93 : // @User: Standard
94 :
95 : // @Param: _RATE_FF
96 : // @DisplayName: Pitch axis rate controller feed forward
97 : // @Description: Pitch axis rate controller feed forward
98 : // @Range: 0 3.0
99 : // @Increment: 0.001
100 : // @User: Standard
101 :
102 : // @Param: _RATE_FLTT
103 : // @DisplayName: Pitch axis rate controller target frequency in Hz
104 : // @Description: Pitch axis rate controller target frequency in Hz
105 : // @Range: 2 50
106 : // @Increment: 1
107 : // @Units: Hz
108 : // @User: Standard
109 :
110 : // @Param: _RATE_FLTE
111 : // @DisplayName: Pitch axis rate controller error frequency in Hz
112 : // @Description: Pitch axis rate controller error frequency in Hz
113 : // @Range: 2 50
114 : // @Increment: 1
115 : // @Units: Hz
116 : // @User: Standard
117 :
118 : // @Param: _RATE_FLTD
119 : // @DisplayName: Pitch axis rate controller derivative frequency in Hz
120 : // @Description: Pitch axis rate controller derivative frequency in Hz
121 : // @Range: 0 50
122 : // @Increment: 1
123 : // @Units: Hz
124 : // @User: Standard
125 :
126 : // @Param: _RATE_SMAX
127 : // @DisplayName: Pitch slew rate limit
128 : // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
129 : // @Range: 0 200
130 : // @Increment: 0.5
131 : // @User: Advanced
132 :
133 : // @Param: _RATE_PDMX
134 : // @DisplayName: Pitch axis rate controller PD sum maximum
135 : // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
136 : // @Range: 0 1
137 : // @Increment: 0.01
138 :
139 : // @Param: _RATE_D_FF
140 : // @DisplayName: Pitch Derivative FeedForward Gain
141 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
142 : // @Range: 0 0.03
143 : // @Increment: 0.001
144 : // @User: Advanced
145 :
146 : // @Param: _RATE_NTF
147 : // @DisplayName: Pitch Target notch filter index
148 : // @Description: Pitch Target notch filter index, zero disables
149 : // @Range: 0 8
150 : // @User: Advanced
151 :
152 : // @Param: _RATE_NEF
153 : // @DisplayName: Pitch Error notch filter index
154 : // @Description: Pitch Error notch filter index, zero disables
155 : // @Range: 0 8
156 : // @User: Advanced
157 :
158 : AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),
159 :
160 : // @Param: 2SRV_ACCEL
161 : // @DisplayName: Pitch max acceleration
162 : // @Description: Pitch acceleration limit. Setting to zero disables input shaping.
163 : // @Range: 0 2500
164 : // @Units: deg/s/s
165 : // @Increment: 1
166 : // @User: Advanced
167 : AP_GROUPINFO("2SRV_ACCEL", 12, AP_PitchController, accel_limit, 500),
168 :
169 : // @Param: _ANGLE_P
170 : // @DisplayName: Pitch angle P gain
171 : // @Description: Pitch angle P gain. If zero a gain of (1 / PTCH2SRV_TCONST) will be used.
172 : // @Range: 0.000 12.000
173 : // @Increment: 0.01
174 : // @User: Advanced
175 : AP_GROUPINFO("_ANGLE_P", 13, AP_PitchController, angle_p, 0.0),
176 :
177 : AP_GROUPEND
178 : };
179 :
180 692 : AP_PitchController::AP_PitchController(const AP_FixedWing &parms)
181 : : AP_FW_Controller(parms,
182 1384 : AC_PID::Defaults{
183 : .p = 0.04,
184 : .i = 0.15,
185 : .d = 0.0,
186 : .ff = 0.345,
187 : .imax = 0.666,
188 : .filt_T_hz = 3.0,
189 : .filt_E_hz = 0.0,
190 : .filt_D_hz = 12.0,
191 : .srmax = 150.0,
192 : .srtau = 1.0
193 : },
194 692 : AP_AutoTune::ATType::AUTOTUNE_PITCH)
195 : {
196 692 : AP_Param::setup_object_defaults(this, var_info);
197 692 : }
198 :
199 : // Return the measured pitch angle in degrees
200 8376783 : float AP_PitchController::get_measured_angle_deg() const
201 : {
202 8376783 : return AP::ahrs().get_pitch_deg();
203 : }
204 :
205 : // Return the measured pitch rate in radians per second
206 8376359 : float AP_PitchController::get_measured_rate_rads() const
207 : {
208 8376359 : return AP::ahrs().get_gyro().y;
209 : }
210 :
211 : // Return true if the airspeed should be considered as under speed
212 7561425 : bool AP_PitchController::is_underspeed() const
213 : {
214 7561425 : return get_airspeed() <= 0.5*float(aparm.airspeed_min);
215 : }
216 :
217 : // Return true if the vehicle is inverted
218 18848390 : bool AP_PitchController::is_inverted() const
219 : {
220 18848390 : return fabsf(AP::ahrs().get_roll_deg()) >= 90.0;
221 : }
222 :
223 : // Return positive rate limit in deg per second, zero if disabled
224 13652895 : float AP_PitchController::get_positive_rate_limit_degs() const
225 : {
226 13652895 : return MAX(gains.rmax_pos.get(), 0.0);
227 : }
228 :
229 : // Return negative rate limit in deg per second (as a positive number) zero if disabled
230 13652895 : float AP_PitchController::get_negative_rate_limit_degs() const
231 : {
232 13652895 : return MAX(gains.rmax_neg.get(), 0.0);
233 : }
234 :
235 : // Return true if rate limits should be applied
236 8156910 : bool AP_PitchController::should_apply_rate_limits() const
237 : {
238 8156910 : return !is_inverted();
239 : }
240 :
241 : // get the rate offset in degrees/second needed for pitch in body frame to maintain height in a coordinated turn.
242 5345740 : float AP_PitchController::get_rate_target_offset_degs() const
243 : {
244 5345740 : const AP_AHRS &_ahrs = AP::ahrs();
245 :
246 5345740 : float bank_angle = _ahrs.get_roll_rad();
247 :
248 : // limit bank angle between +- 80 deg if right way up and between 100 and 260 if inverted
249 5345740 : if (!is_inverted()) {
250 5276536 : bank_angle = constrain_float(bank_angle,-radians(80),radians(80));
251 : } else {
252 : // Note that the wrap means we have a different range here, we could wrap it back but its only used in trigonometric functions so we don't need to.
253 69204 : bank_angle = constrain_float(wrap_2PI(bank_angle), radians(100), radians(260));
254 : }
255 5345740 : if (abs(_ahrs.pitch_sensor) > 7000) {
256 : // don't do turn coordination handling when at very high pitch angles
257 97713 : return 0.0;
258 : }
259 :
260 : // Assume true airspeed is at least min airspeed, protect against zeros.
261 5248027 : const float true_airspeed = MAX((get_airspeed() * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1));
262 :
263 : // Lateral acceleration
264 5248027 : const float lateral_accel = tanf(bank_angle) * GRAVITY_MSS * cosf(_ahrs.get_pitch_rad());
265 :
266 : // Resultant turn rate in the pitch axis
267 5248027 : const float turn_rate = (lateral_accel / true_airspeed) * sinf(bank_angle);
268 :
269 : // Apply gain
270 5248027 : float rate_offset = fabsf(degrees(turn_rate)) * _roll_ff;
271 :
272 5248027 : return rate_offset;
273 : }
274 :
275 : // Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
276 : // A positive demand is up
277 5345740 : float AP_PitchController::run_axis_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode)
278 : {
279 : // Invert desired if vehicle is inverted.
280 5345740 : if (is_inverted()) {
281 69204 : desired_rate_degs *= -1.0;
282 : }
283 :
284 : /*
285 : when we are past the users defined roll limit for the aircraft
286 : our priority should be to bring the aircraft back within the
287 : roll limit. Using elevator for pitch control at large roll
288 : angles is ineffective, and can be counter productive as it
289 : induces earth-frame yaw which can reduce the ability to roll. We
290 : linearly reduce pitch demanded rate when beyond the configured
291 : roll limit, reducing to zero at 90 degrees
292 : */
293 5345740 : const AP_AHRS &_ahrs = AP::ahrs();
294 5345740 : float roll_wrapped = labs(_ahrs.roll_sensor);
295 5345740 : if (roll_wrapped > 9000) {
296 69191 : roll_wrapped = 18000 - roll_wrapped;
297 : }
298 5345740 : const float roll_limit_margin = MIN(aparm.roll_limit*100 + 500.0, 8500.0);
299 5345740 : if (roll_wrapped > roll_limit_margin && labs(_ahrs.pitch_sensor) < 7000) {
300 8677 : float roll_prop = (roll_wrapped - roll_limit_margin) / (float)(9000 - roll_limit_margin);
301 8677 : desired_rate_degs *= (1 - roll_prop);
302 : }
303 :
304 5345740 : return run_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
305 : }
|
