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 Andrew Tridgell
17 : // Based upon the roll controller by Paul Riseborough and Jon Challinger
18 : //
19 :
20 : #include <AP_AHRS/AP_AHRS.h>
21 : #include <AP_Math/AP_Math.h>
22 : #include <AP_HAL/AP_HAL.h>
23 : #include "AP_SteerController.h"
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : const AP_Param::GroupInfo AP_SteerController::var_info[] = {
28 : // @Param: TCONST
29 : // @DisplayName: Steering Time Constant
30 : // @Description: This controls the time constant in seconds from demanded to achieved steering angle. A value of 0.75 is a good default and will work with nearly all rovers. Ground steering in aircraft needs a bit smaller time constant, and a value of 0.5 is recommended for best ground handling in fixed wing aircraft. A value of 0.75 means that the controller will try to correct any deviation between the desired and actual steering angle in 0.75 seconds. Advanced users may want to reduce this time to obtain a faster response but there is no point setting a time less than the vehicle can achieve.
31 : // @Range: 0.4 1.0
32 : // @Units: s
33 : // @Increment: 0.1
34 : // @User: Advanced
35 : AP_GROUPINFO("TCONST", 0, AP_SteerController, _tau, 0.75f),
36 :
37 : // @Param: P
38 : // @DisplayName: Steering turning gain
39 : // @Description: The proportional gain for steering. This should be approximately equal to the diameter of the turning circle of the vehicle at low speed and maximum steering angle
40 : // @Range: 0.1 10.0
41 : // @Increment: 0.1
42 : // @User: Standard
43 : AP_GROUPINFO("P", 1, AP_SteerController, _K_P, 1.8f),
44 :
45 : // @Param: I
46 : // @DisplayName: Integrator Gain
47 : // @Description: This is the gain from the integral of steering angle. Increasing this gain causes the controller to trim out steady offsets due to an out of trim vehicle.
48 : // @Range: 0 1.0
49 : // @Increment: 0.05
50 : // @User: Standard
51 : AP_GROUPINFO("I", 3, AP_SteerController, _K_I, 0.2f),
52 :
53 : // @Param: D
54 : // @DisplayName: Damping Gain
55 : // @Description: This adjusts the damping of the steering control loop. This gain helps to reduce steering jitter with vibration. It should be increased in 0.01 increments as too high a value can lead to a high frequency steering oscillation that could overstress the vehicle.
56 : // @Range: 0 0.1
57 : // @Increment: 0.01
58 : // @User: Standard
59 : AP_GROUPINFO("D", 4, AP_SteerController, _K_D, 0.005f),
60 :
61 : // @Param: IMAX
62 : // @DisplayName: Integrator limit
63 : // @Description: This limits the number of degrees of steering in centi-degrees over which the integrator will operate. At the default setting of 1500 centi-degrees, the integrator will be limited to +- 15 degrees of servo travel. The maximum servo deflection is +- 45 centi-degrees, so the default value represents a 1/3rd of the total control throw which is adequate unless the vehicle is severely out of trim.
64 : // @Range: 0 4500
65 : // @Increment: 10
66 : // @Units: cdeg
67 : // @User: Advanced
68 : AP_GROUPINFO("IMAX", 5, AP_SteerController, _imax, 1500),
69 :
70 : // @Param: MINSPD
71 : // @DisplayName: Minimum speed
72 : // @Description: This is the minimum assumed ground speed in meters/second for steering. Having a minimum speed prevents oscillations when the vehicle first starts moving. The vehicle can still drive slower than this limit, but the steering calculations will be done based on this minimum speed.
73 : // @Range: 0 5
74 : // @Increment: 0.1
75 : // @Units: m/s
76 : // @User: Standard
77 : AP_GROUPINFO("MINSPD", 6, AP_SteerController, _minspeed, 1.0f),
78 :
79 :
80 : // @Param: FF
81 : // @DisplayName: Steering feed forward
82 : // @Description: The feed forward gain for steering this is the ratio of the achieved turn rate to applied steering. A value of 1 means that the vehicle would yaw at a rate of 45 degrees per second with full steering deflection at 1m/s ground speed.
83 : // @Range: 0.0 10.0
84 : // @Increment: 0.1
85 : // @User: Standard
86 : AP_GROUPINFO("FF", 7, AP_SteerController, _K_FF, 0),
87 :
88 :
89 : // @Param: DRTSPD
90 : // @DisplayName: Derating speed
91 : // @Description: Speed after that the maximum degree of steering will start to derate. Set this speed to a maximum speed that a plane can do controlled turn at maximum angle of steering wheel without rolling to wing. If 0 then no derating is used.
92 : // @Range: 0.0 30.0
93 : // @Increment: 0.1
94 : // @Units: m/s
95 : // @User: Advanced
96 : AP_GROUPINFO("DRTSPD", 8, AP_SteerController, _deratespeed, 0),
97 :
98 : // @Param: DRTFCT
99 : // @DisplayName: Derating factor
100 : // @Description: Degrees of steering wheel to derate at each additional m/s of speed above "Derating speed". Should be set so that at higher speeds the plane does not roll to the wing in turns.
101 : // @Range: 0.0 50.0
102 : // @Increment: 0.1
103 : // @Units: deg/m/s
104 : // @User: Advanced
105 : AP_GROUPINFO("DRTFCT", 9, AP_SteerController, _deratefactor, 10),
106 :
107 : // @Param: DRTMIN
108 : // @DisplayName: Minimum angle of wheel
109 : // @Description: The angle that limits smallest angle of steering wheel at maximum speed. Even if it should derate below, it will stop derating at this angle.
110 : // @Range: 0 4500
111 : // @Increment: 10
112 : // @Units: cdeg
113 : // @User: Advanced
114 : AP_GROUPINFO("DRTMIN", 10, AP_SteerController, _mindegree, 4500),
115 :
116 : AP_GROUPEND
117 : };
118 :
119 :
120 : /*
121 : steering rate controller. Returns servo out -4500 to 4500 given
122 : desired yaw rate in degrees/sec. Positive yaw rate means clockwise yaw.
123 : */
124 123090 : int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
125 : {
126 123090 : uint32_t tnow = AP_HAL::millis();
127 123090 : uint32_t dt = tnow - _last_t;
128 123090 : if (_last_t == 0 || dt > 1000) {
129 110 : dt = 0;
130 : }
131 123090 : _last_t = tnow;
132 :
133 123090 : AP_AHRS &_ahrs = AP::ahrs();
134 :
135 123090 : float speed = _ahrs.groundspeed();
136 123090 : if (speed < _minspeed) {
137 : // assume a minimum speed. This stops oscillations when first starting to move
138 29127 : speed = _minspeed;
139 : }
140 :
141 : // this is a linear approximation of the inverse steering
142 : // equation for a ground vehicle. It returns steering as an angle from -45 to 45
143 123090 : float scaler = 1.0f / speed;
144 :
145 123090 : _pid_info.target = desired_rate;
146 :
147 : // Calculate the steering rate error (deg/sec) and apply gain scaler
148 : // We do this in earth frame to allow for rover leaning over in hard corners
149 123090 : float yaw_rate_earth = degrees(_ahrs.get_yaw_rate_earth());
150 123090 : if (_reverse) {
151 0 : yaw_rate_earth *= -1.0f;
152 : }
153 123090 : _pid_info.actual = yaw_rate_earth;
154 :
155 123090 : float rate_error = (desired_rate - yaw_rate_earth) * scaler;
156 :
157 : // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
158 : // No conversion is required for K_D
159 123090 : float ki_rate = _K_I * _tau * 45.0f;
160 123090 : float kp_ff = MAX((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f;
161 123090 : float k_ff = _K_FF * 45.0f;
162 123090 : float delta_time = (float)dt * 0.001f;
163 :
164 : // Multiply yaw rate error by _ki_rate and integrate
165 : // Don't integrate if in stabilize mode as the integrator will wind up against the pilots inputs
166 123090 : if (ki_rate > 0 && speed >= _minspeed) {
167 : // only integrate if gain and time step are positive.
168 123090 : if (dt > 0) {
169 122980 : float integrator_delta = rate_error * ki_rate * delta_time * scaler;
170 : // prevent the integrator from increasing if steering defln demand is above the upper limit
171 122980 : if (_last_out < -45) {
172 4364 : integrator_delta = MAX(integrator_delta , 0);
173 118616 : } else if (_last_out > 45) {
174 : // prevent the integrator from decreasing if steering defln demand is below the lower limit
175 2902 : integrator_delta = MIN(integrator_delta, 0);
176 : }
177 122980 : _pid_info.I += integrator_delta;
178 : }
179 : } else {
180 0 : _pid_info.I = 0;
181 : }
182 :
183 : // Scale the integration limit
184 123090 : float intLimScaled = _imax * 0.01f;
185 :
186 : // Constrain the integrator state
187 123090 : _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
188 :
189 123090 : _pid_info.D = rate_error * _K_D * 4.0f;
190 123090 : _pid_info.P = (radians(desired_rate) * kp_ff) * scaler;
191 123090 : _pid_info.FF = (radians(desired_rate) * k_ff) * scaler;
192 :
193 : // Calculate the demanded control surface deflection
194 123090 : _last_out = _pid_info.D + _pid_info.FF + _pid_info.P + _pid_info.I;
195 :
196 123090 : float derate_constraint = 4500;
197 :
198 : // Calculate required constrain based on speed
199 123090 : if (!is_zero(_deratespeed) && speed > _deratespeed) {
200 0 : derate_constraint = 4500 - (speed - _deratespeed) * _deratefactor * 100;
201 0 : if (derate_constraint < _mindegree) {
202 0 : derate_constraint = _mindegree;
203 : }
204 : }
205 :
206 : // Convert to centi-degrees and constrain
207 123090 : return constrain_float(_last_out * 100, -derate_constraint, derate_constraint);
208 : }
209 :
210 :
211 : /*
212 : lateral acceleration controller. Returns servo value -4500 to 4500
213 : given a desired lateral acceleration
214 : */
215 0 : int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
216 : {
217 0 : float speed = AP::ahrs().groundspeed();
218 0 : if (speed < _minspeed) {
219 : // assume a minimum speed. This reduces osciallations when first starting to move
220 0 : speed = _minspeed;
221 : }
222 :
223 : // Calculate the desired steering rate given desired_accel and speed
224 0 : float desired_rate = degrees(desired_accel / speed);
225 0 : if (_reverse) {
226 0 : desired_rate *= -1;
227 : }
228 0 : return get_steering_out_rate(desired_rate);
229 : }
230 :
231 : /*
232 : return a steering servo value from -4500 to 4500 given an angular
233 : steering error in centidegrees.
234 : */
235 123090 : int32_t AP_SteerController::get_steering_out_angle_error(int32_t angle_err)
236 : {
237 123090 : if (_tau < 0.1f) {
238 0 : _tau.set(0.1f);
239 : }
240 :
241 : // Calculate the desired steering rate (deg/sec) from the angle error
242 123090 : float desired_rate = angle_err * 0.01f / _tau;
243 :
244 123090 : return get_steering_out_rate(desired_rate);
245 : }
246 :
247 1655051 : void AP_SteerController::reset_I()
248 : {
249 1655051 : _pid_info.I = 0;
250 1655051 : }
251 :
252 : // Returns true if controller has been run recently
253 1016541 : bool AP_SteerController::active() const
254 : {
255 1016541 : return (AP_HAL::millis() - _last_t) < 1000;
256 : }
257 :
|
