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 : support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall
17 : Converted to a library by Andrew Tridgell
18 : */
19 : #pragma once
20 :
21 : #include "AC_AutoTune_config.h"
22 :
23 : #if AC_AUTOTUNE_ENABLED
24 :
25 : #include <AC_AttitudeControl/AC_AttitudeControl.h>
26 : #include <AC_AttitudeControl/AC_PosControl.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <RC_Channel/RC_Channel.h>
29 : #include "AC_AutoTune_FreqResp.h"
30 :
31 : #define AUTOTUNE_AXIS_BITMASK_ROLL 1
32 : #define AUTOTUNE_AXIS_BITMASK_PITCH 2
33 : #define AUTOTUNE_AXIS_BITMASK_YAW 4
34 : #define AUTOTUNE_AXIS_BITMASK_YAW_D 8
35 :
36 : #define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains
37 :
38 : // Auto Tune message ids for ground station
39 : #define AUTOTUNE_MESSAGE_STARTED 0
40 : #define AUTOTUNE_MESSAGE_STOPPED 1
41 : #define AUTOTUNE_MESSAGE_SUCCESS 2
42 : #define AUTOTUNE_MESSAGE_FAILED 3
43 : #define AUTOTUNE_MESSAGE_SAVED_GAINS 4
44 : #define AUTOTUNE_MESSAGE_TESTING 5
45 : #define AUTOTUNE_MESSAGE_TESTING_END 6
46 :
47 : #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
48 :
49 : class AC_AutoTune
50 : {
51 : public:
52 : // constructor
53 : AC_AutoTune();
54 :
55 : // Main update loop for Autotune mode. Handles all states: tuning, testing, or idle.
56 : // Should be called at >=100Hz for reliable performance.
57 : virtual void run();
58 :
59 : // Possibly save gains, called on disarm
60 : void disarmed(const bool in_autotune_mode);
61 :
62 : // stop tune, reverting gains
63 : void stop();
64 :
65 : // Autotune aux function trigger
66 : void do_aux_function(const RC_Channel::AuxSwitchPos ch_flag);
67 :
68 : protected:
69 :
70 : virtual void save_tuning_gains() = 0;
71 :
72 :
73 : // reset Autotune so that gains are not saved again and autotune can be run again.
74 943 : void reset() {
75 943 : mode = TuneMode::UNINITIALISED;
76 943 : axes_completed = 0;
77 943 : testing_switch_used = false;
78 943 : }
79 :
80 : // axis that can be tuned
81 : enum class AxisType {
82 : ROLL = 0, // roll axis is being tuned (either angle or rate)
83 : PITCH = 1, // pitch axis is being tuned (either angle or rate)
84 : YAW = 2, // yaw axis is being tuned using FLTE (either angle or rate)
85 : YAW_D = 3, // yaw axis is being tuned using D (either angle or rate)
86 : };
87 :
88 : //
89 : // methods that must be supplied by the vehicle specific subclass
90 : //
91 : virtual bool init(void) = 0;
92 :
93 : // get pilot input for desired climb rate
94 : virtual float get_desired_climb_rate_ms(void) const = 0;
95 :
96 : // get pilot input for designed roll and pitch, and yaw rate
97 : virtual void get_pilot_desired_rp_yrate_rad(float &roll_rad, float &pitch_rad, float &yaw_rate_rads) = 0;
98 :
99 : // init pos controller Z velocity and accel limits
100 : virtual void init_z_limits() = 0;
101 :
102 : #if HAL_LOGGING_ENABLED
103 : // log PIDs at full rate for during test
104 : virtual void log_pids() = 0;
105 : #endif
106 :
107 : //
108 : // methods to load and save gains
109 : //
110 :
111 : // backup original gains and prepare for start of tuning
112 : virtual void backup_gains_and_initialise();
113 :
114 : // switch to use original gains
115 : virtual void load_orig_gains() = 0;
116 :
117 : // switch to gains found by last successful autotune
118 : virtual void load_tuned_gains() = 0;
119 :
120 : // load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
121 : virtual void load_intra_test_gains() = 0;
122 :
123 : // load gains for next test. relies on axis variable being set
124 : virtual void load_test_gains() = 0;
125 :
126 : // reset the test variables for each vehicle
127 : virtual void reset_vehicle_test_variables() = 0;
128 :
129 : // reset the update gain variables for each vehicle
130 : virtual void reset_update_gain_variables() = 0;
131 :
132 : // test initialization and run methods that should be overridden for each vehicle
133 : virtual void test_init() = 0;
134 : virtual void test_run(AxisType test_axis, const float dir_sign) = 0;
135 :
136 : // return true if user has enabled autotune for roll, pitch or yaw axis
137 : bool roll_enabled() const;
138 : bool pitch_enabled() const;
139 : bool yaw_enabled() const;
140 : bool yaw_d_enabled() const;
141 :
142 : // update gains for the rate p up tune type
143 : virtual void updating_rate_p_up_all(AxisType test_axis)=0;
144 :
145 : // update gains for the rate d up tune type
146 : virtual void updating_rate_d_up_all(AxisType test_axis)=0;
147 :
148 : // update gains for the rate d down tune type
149 : virtual void updating_rate_d_down_all(AxisType test_axis)=0;
150 :
151 : // update gains for the angle p up tune type
152 : virtual void updating_angle_p_up_all(AxisType test_axis)=0;
153 :
154 : // update gains for the angle p down tune type
155 : virtual void updating_angle_p_down_all(AxisType test_axis)=0;
156 :
157 : // set gains post tune for the tune type
158 : virtual void set_tuning_gains_with_backoff(AxisType test_axis)=0;
159 :
160 : // reverse the direction of the next test
161 : virtual bool reverse_test_direction() = 0;
162 :
163 :
164 : #if HAL_LOGGING_ENABLED
165 : virtual void Log_AutoTune() = 0;
166 : virtual void Log_AutoTuneDetails() = 0;
167 : virtual void Log_AutoTuneSweep() = 0;
168 : #endif
169 :
170 : // internal init function, should be called from init()
171 : bool init_internals(bool use_poshold,
172 : AC_AttitudeControl *attitude_control,
173 : AC_PosControl *pos_control,
174 : AP_AHRS_View *ahrs_view);
175 :
176 : // send intermittent updates to user on status of tune
177 : virtual void do_gcs_announcements() = 0;
178 :
179 : // send post test updates to user
180 : virtual void do_post_test_gcs_announcements() = 0;
181 :
182 : // send message with high level status (e.g. Started, Stopped)
183 : void update_gcs(uint8_t message_id) const;
184 :
185 : // send lower level step status (e.g. Pilot overrides Active)
186 : void send_step_string();
187 :
188 : // convert tune type to string for reporting
189 : const char *get_tune_type_name() const;
190 :
191 : // return current axis string
192 : const char *get_axis_name() const;
193 :
194 : // report final gains for a given axis to GCS
195 : virtual void report_final_gains(AxisType test_axis) const = 0;
196 :
197 : // Functions added for heli autotune
198 :
199 : // Add additional updating gain functions specific to heli
200 : // generic method used by subclasses to update gains for the rate ff up tune type
201 : virtual void updating_rate_ff_up_all(AxisType test_axis)=0;
202 :
203 : // generic method used by subclasses to update gains for the max gain tune type
204 : virtual void updating_max_gains_all(AxisType test_axis)=0;
205 :
206 : // steps performed while in the tuning mode
207 : enum class Step {
208 : WAITING_FOR_LEVEL = 0, // Waiting for the vehicle to stabilize at level before starting a test.
209 : EXECUTING_TEST = 1, // Performing a test and monitoring the vehicle's response.
210 : UPDATE_GAINS = 2, // Updating gains based on test results.
211 : ABORT = 3 // Aborting the current test; revert to safe gains and return to WAITING_FOR_LEVEL.
212 : };
213 : Step step; // see StepType for what steps are performed
214 :
215 : // mini steps performed while in Tuning mode, Testing step
216 : enum class TuneType {
217 : RATE_D_UP = 0, // rate D is being tuned up
218 : RATE_D_DOWN = 1, // rate D is being tuned down
219 : RATE_P_UP = 2, // rate P is being tuned up
220 : RATE_FF_UP = 3, // rate FF is being tuned up
221 : ANGLE_P_DOWN = 4, // angle P is being tuned down
222 : ANGLE_P_UP = 5, // angle P is being tuned up
223 : MAX_GAINS = 6, // max allowable stable gains are determined
224 : TUNE_CHECK = 7, // frequency sweep with tuned gains
225 : TUNE_COMPLETE = 8 // Reached end of tuning
226 : };
227 : TuneType tune_type; // see TuneType
228 : TuneType tune_seq[6]; // holds sequence of tune_types to be performed
229 : uint8_t tune_seq_index; // current tune sequence step
230 :
231 : // get the next tune type
232 : void next_tune_type(TuneType &curr_tune_type, bool reset);
233 :
234 : // Sets customizable tune sequence for the vehicle
235 : virtual void set_tune_sequence() = 0;
236 :
237 : // get_axis_bitmask accessor
238 : virtual uint8_t get_axis_bitmask() const = 0;
239 :
240 : // get_testing_step_timeout_ms accessor
241 : virtual uint32_t get_testing_step_timeout_ms() const = 0;
242 :
243 : // get attitude for slow position hold in autotune mode
244 : void get_poshold_attitude_rad(float &roll_rad, float &pitch_rad, float &yaw_rad);
245 :
246 : // type of gains to load
247 : enum class GainType {
248 : ORIGINAL = 0, // Gains as configured before autotune started
249 : TEST = 1, // Gains applied during an active test
250 : INTRA_TEST = 2, // Gains applied between tests to maintain safe control while returning to level, with slower I-term buildup
251 : TUNED = 3, // Gains discovered by the autotune process, used for flight testing or final use
252 : } loaded_gains;
253 : void load_gains(enum GainType gain_type);
254 :
255 : // TuneMode defines the high-level state of the autotune process.
256 : enum class TuneMode {
257 : UNINITIALISED = 0, // Autotune has not yet been started.
258 : TUNING = 1, // Autotune is actively running and tuning PID gains.
259 : FINISHED = 2, // Tuning is complete, original (pre-tune) gains are restored.
260 : FAILED = 3, // Tuning failed, vehicle is flying with original gains.
261 : VALIDATING = 4, // Tuning complete, user is flight testing the newly tuned gains.
262 : };
263 : TuneMode mode; // see TuneMode for what modes are allowed
264 :
265 : // copies of object pointers to make code a bit clearer
266 : AC_AttitudeControl *attitude_control;
267 : AC_PosControl *pos_control;
268 : AP_AHRS_View *ahrs_view;
269 : AP_Motors *motors;
270 :
271 : AxisType axis; // current axis being tuned. see AxisType enum
272 : bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll)
273 : bool angle_step_commanded; // true on first iteration of the tests (used to signal we must step the attitude or rate target)
274 : uint8_t axes_completed; // bitmask of completed axes
275 : uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks)
276 : uint32_t step_timeout_ms; // time limit of current autotune process
277 : uint32_t level_start_time_ms; // start time of waiting for level
278 : int8_t success_counter; // counter for tuning gains
279 : float start_angle; // start angle
280 : float start_rate; // start rate - parent and multi
281 : float test_accel_max_cdss; // maximum acceleration variable
282 : float step_scaler; // scaler to reduce maximum target step - parent and multi
283 :
284 : LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
285 :
286 : // backup of currently being tuned parameter values
287 : float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel_radss, orig_roll_rate_rads;
288 : float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_dff, orig_pitch_fltt, orig_pitch_smax, orig_pitch_sp, orig_pitch_accel_radss, orig_pitch_rate_rads;
289 : float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_dff, orig_yaw_fltt, orig_yaw_smax, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel_radss, orig_yaw_rate_rads;
290 : bool orig_bf_feedforward;
291 :
292 : // currently being tuned parameter values
293 : float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel_radss;
294 : float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel_radss;
295 : float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel_radss;
296 : float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff;
297 :
298 : uint32_t last_announce_ms;
299 : float lean_angle;
300 : float rotation_rate;
301 : float desired_roll_rad, desired_pitch_rad, desired_yaw_rad; // desired attitude target setpoints and test origins
302 :
303 : // heli specific variables
304 : float start_freq; //start freq for dwell test
305 : float stop_freq; //ending freq for dwell test
306 :
307 : private:
308 : // return true if we have a good position estimate
309 : virtual bool position_ok();
310 :
311 : // methods subclasses must implement to specify max/min test angles:
312 : virtual float target_angle_max_rp_cd() const = 0;
313 :
314 : // methods subclasses must implement to specify max/min test angles:
315 : virtual float target_angle_max_y_cd() const = 0;
316 :
317 : // methods subclasses must implement to specify max/min test angles:
318 : virtual float target_angle_min_rp_cd() const = 0;
319 :
320 : // methods subclasses must implement to specify max/min test angles:
321 : virtual float target_angle_min_y_cd() const = 0;
322 :
323 : // methods subclasses must implement to specify max/min test angles:
324 : virtual float angle_lim_max_rp_cd() const = 0;
325 :
326 : // methods subclasses must implement to specify max/min test angles:
327 : virtual float angle_lim_neg_rpy_cd() const = 0;
328 :
329 : // initialise position controller
330 : bool init_position_controller();
331 :
332 : // Main tuning state machine. Handles WAITING_FOR_LEVEL, EXECUTING_TEST, UPDATE_GAINS, ABORT.
333 : // Updates attitude controller targets and evaluates test responses to tune gains.
334 : void control_attitude();
335 :
336 : // returns true if vehicle is close to level
337 : bool currently_level();
338 :
339 : bool pilot_override; // true = pilot is overriding controls so we suspend tuning temporarily
340 : bool use_poshold; // true = enable position hold
341 : bool have_position; // true = start_position is value
342 : Vector3p start_position_ned_m; // target when holding position as an offset from EKF origin in meters in NED frame
343 :
344 : // variables
345 : uint32_t override_time; // the last time the pilot overrode the controls
346 :
347 : // time in ms of last pilot override warning
348 : uint32_t last_pilot_override_warning;
349 :
350 : // True if we ever got a pilot testing command of tuned gains.
351 : // If true then disarming will save if the tuned gains are currently active.
352 : bool testing_switch_used;
353 :
354 : };
355 :
356 : #endif // AC_AUTOTUNE_ENABLED
|
