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 : // Code by Jon Challinger
17 : // Modified by Paul Riseborough
18 : //
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include "AP_RollController.h"
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <AP_Scheduler/AP_Scheduler.h>
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : const AP_Param::GroupInfo AP_RollController::var_info[] = {
28 : // @Param: 2SRV_TCONST
29 : // @DisplayName: Roll Time Constant
30 : // @Description: Time constant in seconds from demanded to achieved roll 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_RollController, gains.tau, 0.5f),
36 :
37 : // index 1 to 3 reserved for old PID values
38 :
39 : // @Param: 2SRV_RMAX
40 : // @DisplayName: Maximum Roll Rate
41 : // @Description: This sets the maximum roll rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables this limit.
42 : // @Range: 0 180
43 : // @Units: deg/s
44 : // @Increment: 1
45 : // @User: Advanced
46 : AP_GROUPINFO("2SRV_RMAX", 4, AP_RollController, gains.rmax_pos, 0),
47 :
48 : // index 5, 6 reserved for old IMAX, FF
49 :
50 : // @Param: _RATE_P
51 : // @DisplayName: Roll axis rate controller P gain
52 : // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
53 : // @Range: 0.08 0.35
54 : // @Increment: 0.005
55 : // @User: Standard
56 :
57 : // @Param: _RATE_I
58 : // @DisplayName: Roll axis rate controller I gain
59 : // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
60 : // @Range: 0.01 0.6
61 : // @Increment: 0.01
62 : // @User: Standard
63 :
64 : // @Param: _RATE_IMAX
65 : // @DisplayName: Roll axis rate controller I gain maximum
66 : // @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
67 : // @Range: 0 1
68 : // @Increment: 0.01
69 : // @User: Standard
70 :
71 : // @Param: _RATE_D
72 : // @DisplayName: Roll axis rate controller D gain
73 : // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
74 : // @Range: 0.001 0.03
75 : // @Increment: 0.001
76 : // @User: Standard
77 :
78 : // @Param: _RATE_FF
79 : // @DisplayName: Roll axis rate controller feed forward
80 : // @Description: Roll axis rate controller feed forward
81 : // @Range: 0 3.0
82 : // @Increment: 0.001
83 : // @User: Standard
84 :
85 : // @Param: _RATE_FLTT
86 : // @DisplayName: Roll axis rate controller target frequency in Hz
87 : // @Description: Roll axis rate controller target frequency in Hz
88 : // @Range: 2 50
89 : // @Increment: 1
90 : // @Units: Hz
91 : // @User: Standard
92 :
93 : // @Param: _RATE_FLTE
94 : // @DisplayName: Roll axis rate controller error frequency in Hz
95 : // @Description: Roll axis rate controller error frequency in Hz
96 : // @Range: 2 50
97 : // @Increment: 1
98 : // @Units: Hz
99 : // @User: Standard
100 :
101 : // @Param: _RATE_FLTD
102 : // @DisplayName: Roll axis rate controller derivative frequency in Hz
103 : // @Description: Roll axis rate controller derivative frequency in Hz
104 : // @Range: 0 50
105 : // @Increment: 1
106 : // @Units: Hz
107 : // @User: Standard
108 :
109 : // @Param: _RATE_SMAX
110 : // @DisplayName: Roll slew rate limit
111 : // @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.
112 : // @Range: 0 200
113 : // @Increment: 0.5
114 : // @User: Advanced
115 :
116 : // @Param: _RATE_PDMX
117 : // @DisplayName: Roll axis rate controller PD sum maximum
118 : // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
119 : // @Range: 0 1
120 : // @Increment: 0.01
121 :
122 : // @Param: _RATE_D_FF
123 : // @DisplayName: Roll Derivative FeedForward Gain
124 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
125 : // @Range: 0 0.03
126 : // @Increment: 0.001
127 : // @User: Advanced
128 :
129 : // @Param: _RATE_NTF
130 : // @DisplayName: Roll Target notch filter index
131 : // @Description: Roll Target notch filter index, zero disables
132 : // @Range: 0 8
133 : // @User: Advanced
134 :
135 : // @Param: _RATE_NEF
136 : // @DisplayName: Roll Error notch filter index
137 : // @Description: Roll Error notch filter index, zero disables
138 : // @Range: 0 8
139 : // @User: Advanced
140 :
141 : AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),
142 :
143 : // @Param: 2SRV_ACCEL
144 : // @DisplayName: Roll max acceleration
145 : // @Description: Roll acceleration limit. Setting to zero disables input shaping.
146 : // @Range: 0 2500
147 : // @Units: deg/s/s
148 : // @Increment: 1
149 : // @User: Advanced
150 : AP_GROUPINFO("2SRV_ACCEL", 10, AP_RollController, accel_limit, 500),
151 :
152 : // @Param: _ANGLE_P
153 : // @DisplayName: Roll angle P gain
154 : // @Description: Roll angle P gain. If zero a gain of (1 / RLL2SRV_TCONST) will be used.
155 : // @Range: 0.000 12.000
156 : // @Increment: 0.01
157 : // @User: Advanced
158 : AP_GROUPINFO("_ANGLE_P", 11, AP_RollController, angle_p, 0.0),
159 :
160 : AP_GROUPEND
161 : };
162 :
163 : // constructor
164 692 : AP_RollController::AP_RollController(const AP_FixedWing &parms)
165 : : AP_FW_Controller(parms,
166 1384 : AC_PID::Defaults{
167 : .p = 0.08,
168 : .i = 0.15,
169 : .d = 0.0,
170 : .ff = 0.345,
171 : .imax = 0.666,
172 : .filt_T_hz = 3.0,
173 : .filt_E_hz = 0.0,
174 : .filt_D_hz = 12.0,
175 : .srmax = 150.0,
176 : .srtau = 1.0
177 : },
178 692 : AP_AutoTune::ATType::AUTOTUNE_ROLL)
179 : {
180 692 : AP_Param::setup_object_defaults(this, var_info);
181 692 : }
182 :
183 : // Return the measured roll angle in degrees
184 8378063 : float AP_RollController::get_measured_angle_deg() const
185 : {
186 8378063 : return AP::ahrs().get_roll_deg();
187 : }
188 :
189 : // Return the measured roll rate in radians per second
190 8377639 : float AP_RollController::get_measured_rate_rads() const
191 : {
192 8377639 : return AP::ahrs().get_gyro().x;
193 : }
194 :
195 : // Return true if the airspeed should be considered as under speed
196 7563909 : bool AP_RollController::is_underspeed() const
197 : {
198 7563909 : return get_airspeed() <= float(aparm.airspeed_min);
199 : }
200 :
201 : // Return positive rate limit in deg per second, zero if disabled
202 27179262 : float AP_RollController::get_positive_rate_limit_degs() const
203 : {
204 27179262 : return MAX(gains.rmax_pos.get(), 0.0);
205 : }
206 :
207 : // Return negative rate limit in deg per second (as a positive number) zero if disabled
208 13589631 : float AP_RollController::get_negative_rate_limit_degs() const
209 : {
210 13589631 : return get_positive_rate_limit_degs();
211 : }
212 :
213 : // Return true if rate limits should be applied
214 8225751 : bool AP_RollController::should_apply_rate_limits() const
215 : {
216 8225751 : return !in_recovery;
217 : }
218 :
219 : /*
220 : Function returns an equivalent aileron deflection in centi-degrees in the range from -4500 to 4500
221 : A positive demand is up
222 : */
223 5347552 : float AP_RollController::run_axis_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode)
224 : {
225 : /*
226 : prevent indecision in the roll controller when target roll is
227 : close to 180 degrees from the current roll
228 : */
229 5347552 : const float indecision_threshold_deg = 160;
230 5347552 : const float last_desired_rate = _pid_info.target;
231 5347552 : const float abs_angle_err_deg = fabsf(angle_err_deg);
232 5347552 : if (abs_angle_err_deg > indecision_threshold_deg &&
233 11573 : angle_err_deg <= 180) {
234 11573 : if (desired_rate_degs * last_desired_rate < 0) {
235 2084 : desired_rate_degs = -desired_rate_degs;
236 : // increase the desired rate in proportion to the extra
237 : // angle we are requesting
238 2084 : const float new_angle_err_deg = abs_angle_err_deg + (180 - abs_angle_err_deg)*2;
239 2084 : desired_rate_degs *= new_angle_err_deg / abs_angle_err_deg;
240 : }
241 : }
242 :
243 : // in_recovery flag is only valid for single loop, clear it
244 5347552 : in_recovery = false;
245 :
246 5347552 : return run_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
247 : }
|
