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 helicopters
17 : */
18 :
19 : #pragma once
20 :
21 : #include "AC_AutoTune_config.h"
22 :
23 : #if AC_AUTOTUNE_ENABLED
24 :
25 : #include "AC_AutoTune.h"
26 : #include <AP_Math/chirp.h>
27 : #include <GCS_MAVLink/GCS.h>
28 :
29 : #include <AP_Scheduler/AP_Scheduler.h>
30 :
31 : class AC_AutoTune_Heli : public AC_AutoTune
32 : {
33 : public:
34 : // constructor
35 : AC_AutoTune_Heli();
36 :
37 : // save gained, called on disarm
38 : void save_tuning_gains() override;
39 :
40 : // var_info for holding Parameter information
41 : static const struct AP_Param::GroupInfo var_info[];
42 :
43 : protected:
44 :
45 : //
46 : // methods to load and save gains
47 : //
48 :
49 : // backup original gains and prepare for start of tuning
50 : void backup_gains_and_initialise() override;
51 :
52 : // load gains
53 : void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel_radss, float rate_fltt, float rate_flte, float smax, float max_rate_rads);
54 :
55 : // switch to use original gains
56 : void load_orig_gains() override;
57 :
58 : // switch to gains found by last successful autotune
59 : void load_tuned_gains() override;
60 :
61 : // load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step
62 : void load_intra_test_gains() override;
63 :
64 : // load test gains
65 : void load_test_gains() override;
66 :
67 : // reset the test variables for heli
68 : void reset_vehicle_test_variables() override;
69 :
70 : // reset the update gain variables for heli
71 : void reset_update_gain_variables() override;
72 :
73 : // Prepares all tuning state variables and target values for a new test.
74 : void test_init() override;
75 :
76 : // runs test
77 : void test_run(AxisType test_axis, const float dir_sign) override;
78 :
79 : // update gains for the rate p up tune type
80 : void updating_rate_p_up_all(AxisType test_axis) override;
81 :
82 : // update gains for the rate d up tune type
83 : void updating_rate_d_up_all(AxisType test_axis) override;
84 :
85 : // update gains for the rate d down tune type
86 0 : void updating_rate_d_down_all(AxisType test_axis) override {};
87 :
88 : // update gains for the rate ff up tune type
89 : void updating_rate_ff_up_all(AxisType test_axis) override;
90 :
91 : // update gains for the angle p up tune type
92 : void updating_angle_p_up_all(AxisType test_axis) override;
93 :
94 : // update gains for the angle p down tune type
95 0 : void updating_angle_p_down_all(AxisType test_axis) override {};
96 :
97 : // update gains for the max gain tune type
98 : void updating_max_gains_all(AxisType test_axis) override;
99 :
100 : // set gains post tune for the tune type
101 : void set_tuning_gains_with_backoff(AxisType test_axis) override;
102 :
103 : // reverse the direction of the next test
104 147 : bool reverse_test_direction() override { return positive_direction; }
105 :
106 : #if HAL_LOGGING_ENABLED
107 : // methods to log autotune summary data
108 : void Log_AutoTune() override;
109 : void Log_Write_AutoTune(AxisType _axis, TuneType tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel);
110 :
111 : // methods to log autotune time history results for command, angular rate, and attitude.
112 : void Log_AutoTuneDetails() override;
113 : void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad);
114 :
115 : // methods to log autotune frequency response results
116 : void Log_AutoTuneSweep() override;
117 : void Log_Write_AutoTuneSweep(float freq_mtr, float gain_mtr, float phase_mtr, float freq_tgt, float gain_tgt, float phase_tgt);
118 : #endif
119 :
120 : // send intermittent updates to user on status of tune
121 : void do_gcs_announcements() override;
122 :
123 : // send post test updates to user
124 : void do_post_test_gcs_announcements() override;
125 :
126 : // report final gains for a given axis to GCS
127 : void report_final_gains(AxisType test_axis) const override;
128 :
129 : // set the tuning test sequence
130 : void set_tune_sequence() override;
131 :
132 : // get_axis_bitmask accessor
133 540 : uint8_t get_axis_bitmask() const override { return axis_bitmask; }
134 :
135 : // get_testing_step_timeout_ms accessor
136 : uint32_t get_testing_step_timeout_ms() const override;
137 :
138 : private:
139 : // sweep_info contains information about a specific test's sweep results
140 : struct sweep_info {
141 : float freq;
142 : float gain;
143 : float phase;
144 : };
145 :
146 : // max_gain_data type stores information from the max gain test
147 : struct max_gain_data {
148 : float freq;
149 : float phase;
150 : float gain;
151 : float max_allowed;
152 : };
153 :
154 : // FreqRespCalcType is the type of calculation done for the frequency response
155 : enum FreqRespCalcType {
156 : RATE = 0,
157 : ANGLE = 1,
158 : DRB = 2,
159 : };
160 :
161 : enum FreqRespInput {
162 : MOTOR = 0,
163 : TARGET = 1,
164 : };
165 :
166 : float target_angle_max_rp_cd() const override;
167 :
168 : float target_angle_max_y_cd() const override;
169 :
170 : float target_angle_min_rp_cd() const override;
171 :
172 : float target_angle_min_y_cd() const override;
173 :
174 : float angle_lim_max_rp_cd() const override;
175 :
176 : float angle_lim_neg_rpy_cd() const override;
177 :
178 : // initialize dwell test or angle dwell test variables
179 : void dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type);
180 :
181 : // dwell test used to perform frequency dwells for rate gains
182 : void dwell_test_run(sweep_info &test_data);
183 :
184 : // updating_rate_ff_up - adjust FF to ensure the target is reached
185 : // FF is adjusted until rate requested is achieved
186 : void updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq);
187 :
188 : // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
189 : void updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p);
190 :
191 : // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum
192 : void updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d);
193 :
194 : // updating_angle_p_up - determines maximum angle p gain for pitch and roll
195 : void updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq);
196 :
197 : // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
198 : void updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d);
199 :
200 : // freq_search_for_phase: general search strategy for specified phase. interpolation done once specified phase has been bounded.
201 : bool freq_search_for_phase(sweep_info test, float desired_phase, float freq_incr, sweep_info &est_data, float &new_freq);
202 :
203 : // reset the max_gains update gain variables
204 : void reset_maxgains_update_gain_variables();
205 :
206 : // reset the sweep variables
207 : void reset_sweep_variables();
208 :
209 : // exceeded_freq_range - ensures tuning remains inside frequency range
210 : bool exceeded_freq_range(float frequency);
211 :
212 : // report gain formatting helper
213 : void report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float rate_ff, float angle_P, float max_accel_radss) const;
214 :
215 : // define input type as Dwell or Sweep. Used through entire class
216 : AC_AutoTune_FreqResp::InputType input_type;
217 :
218 : sweep_info curr_data; // frequency response test results
219 : float next_test_freq; // next test frequency for next test cycle setup
220 :
221 : // max gain data for rate p tuning
222 : max_gain_data max_rate_p;
223 : // max gain data for rate d tuning
224 : max_gain_data max_rate_d;
225 :
226 : // updating max gain variables
227 : // flag for finding maximum p gain
228 : bool found_max_p;
229 : // flag for finding maximum d gain
230 : bool found_max_d;
231 :
232 : // updating angle P up variables
233 : float phase_max; // track the maximum phase and freq
234 : float freq_max;
235 : float sp_prev_gain; // previous gain
236 : bool found_max_gain_freq; // flag for finding max gain frequency
237 : bool found_peak; // flag for finding the peak of the gain response
238 :
239 : // updating rate D up
240 : float rd_prev_gain; // previous gain
241 :
242 : // freq search for phase
243 : sweep_info prev_test; // data from previous dwell
244 :
245 : // Dwell Test variables
246 : AC_AutoTune_FreqResp::InputType test_input_type;
247 : FreqRespCalcType test_calc_type;
248 : FreqRespInput test_freq_resp_input;
249 : uint8_t num_dwell_cycles;
250 : float test_start_freq;
251 : float tgt_attitude;
252 :
253 : float pre_calc_cycles; // number of cycles to complete before running frequency response calculations
254 : float command_out; // test axis command output
255 : float filt_target_rate; // filtered target rate
256 : float dwell_start_time_ms; // start time in ms of dwell test
257 :
258 : sweep_info curr_test;
259 : sweep_info curr_test_mtr;
260 : sweep_info curr_test_tgt;
261 :
262 : uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test
263 :
264 : // variables from dwell test
265 : LowPassFilterVector2f filt_att_fdbk_from_velxy_cd;
266 : LowPassFilterFloat filt_command_reading; // filtered command reading to keep oscillation centered
267 : LowPassFilterFloat filt_gyro_reading; // filtered gyro reading to keep oscillation centered
268 : LowPassFilterFloat filt_tgt_rate_reading; // filtered target rate reading to keep oscillation centered
269 :
270 : // trim variables for determining trim state prior to test starting
271 : float trim_yaw_tgt_reading_cd; // trim target yaw reading before starting test
272 : float trim_yaw_heading_reading_cd; // trim heading reading before starting test
273 :
274 : LowPassFilterFloat command_filt; // filtered command - filtering intended to remove noise
275 : LowPassFilterFloat target_rate_filt; // filtered target rate in radians/second - filtering intended to remove noise
276 :
277 : // sweep_data tracks the overall characteristics in the response to the frequency sweep
278 : struct sweep_data {
279 : sweep_info maxgain;
280 : sweep_info ph180;
281 : sweep_info ph270;
282 : uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg;
283 : };
284 : sweep_data sweep_mtr;
285 : sweep_data sweep_tgt;
286 : bool sweep_complete;
287 :
288 : // fix the frequency sweep time to 23 seconds
289 : const float sweep_time_ms = 23000;
290 :
291 : // parameters
292 : AP_Int8 axis_bitmask; // axes to be tuned
293 : AP_Int8 seq_bitmask; // tuning sequence bitmask
294 : AP_Float min_sweep_freq; // minimum sweep frequency
295 : AP_Float max_sweep_freq; // maximum sweep frequency
296 : AP_Float max_resp_gain; // maximum response gain
297 : AP_Float vel_hold_gain; // gain for velocity hold
298 : AP_Float accel_max_degss; // maximum autotune angular acceleration
299 : AP_Float rate_max_degs; // maximum autotune angular rate
300 :
301 : // freqresp object for the frequency response tests
302 : AC_AutoTune_FreqResp freqresp_mtr; // frequency response of output to motor mixer input
303 : AC_AutoTune_FreqResp freqresp_tgt; // frequency response of output to target input
304 :
305 : // allow tracking of cycles complete for frequency response object
306 : bool cycle_complete_tgt;
307 : bool cycle_complete_mtr;
308 :
309 : Chirp chirp_input;
310 : };
311 :
312 : #endif // AC_AUTOTUNE_ENABLED
|
