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 : Multirotor implementation of AutoTune. Based on the original ArduCopter
17 : autotune code by Leonard Hall.
18 : */
19 :
20 : #pragma once
21 :
22 : #include "AC_AutoTune_config.h"
23 :
24 : #if AC_AUTOTUNE_ENABLED
25 :
26 : #include "AC_AutoTune.h"
27 :
28 : class AC_AutoTune_Multi : public AC_AutoTune
29 : {
30 : public:
31 : // Constructor
32 : AC_AutoTune_Multi();
33 :
34 : // Saves tuned gains to EEPROM on disarm
35 : void save_tuning_gains() override;
36 :
37 : // Parameter group for AP_Param registration
38 : static const struct AP_Param::GroupInfo var_info[];
39 :
40 : protected:
41 : //
42 : // Gain management and initialization
43 : //
44 :
45 : // Backs up original gains and resets tuning state
46 : void backup_gains_and_initialise() override;
47 :
48 : // Loads original pre-tune gains
49 : void load_orig_gains() override;
50 :
51 : // Loads gains from the last successful autotune session
52 : void load_tuned_gains() override;
53 :
54 : // Loads conservative gains used between twitch tests
55 : void load_intra_test_gains() override;
56 :
57 : // Loads current test gains before executing a twitch
58 : void load_test_gains() override;
59 :
60 : // Resets vehicle-specific test state (not used in multirotor)
61 0 : void reset_vehicle_test_variables() override {};
62 :
63 : // Resets vehicle-specific gain tracking state (not used in multirotor)
64 14 : void reset_update_gain_variables() override {};
65 :
66 : // Maximum/Minimum target angles for the current axis (in centidegrees)
67 : float target_angle_max_rp_cd() const override;
68 : float target_angle_max_y_cd() const override;
69 : float target_angle_min_rp_cd() const override;
70 : float target_angle_min_y_cd() const override;
71 :
72 : // Absolute and negative angle abort limits for twitch safety (in centidegrees)
73 : float angle_lim_max_rp_cd() const override;
74 : float angle_lim_neg_rpy_cd() const override;
75 :
76 : // Prepares state and targets for a new twitch test
77 : void test_init() override;
78 :
79 : // Executes one twitch step for the specified axis and direction
80 : void test_run(AxisType test_axis, const float dir_sign) override;
81 :
82 : // Sends regular status messages to the ground station
83 : void do_gcs_announcements() override;
84 :
85 : // (Unused) Placeholder for post-test announcements
86 2097 : void do_post_test_gcs_announcements() override {};
87 :
88 : // Reports final tuned gains to the ground station
89 : void report_final_gains(AxisType test_axis) const override;
90 :
91 : // Update functions for each TuneType
92 : void updating_rate_p_up_all(AxisType test_axis) override;
93 : void updating_rate_d_up_all(AxisType test_axis) override;
94 : void updating_rate_d_down_all(AxisType test_axis) override;
95 : void updating_angle_p_up_all(AxisType test_axis) override;
96 : void updating_angle_p_down_all(AxisType test_axis) override;
97 :
98 : // These tune types are not used in multirotors
99 0 : void updating_rate_ff_up_all(AxisType) override {
100 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
101 0 : }
102 0 : void updating_max_gains_all(AxisType) override {
103 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
104 0 : }
105 :
106 : // Applies gain margin (backoff) at the end of a TuneType
107 : void set_tuning_gains_with_backoff(AxisType test_axis) override;
108 :
109 : // reverse the direction of the next test
110 2182 : bool reverse_test_direction() override { return !positive_direction; }
111 :
112 : #if HAL_LOGGING_ENABLED
113 : void Log_AutoTune() override;
114 : void Log_AutoTuneDetails() override;
115 0 : void Log_AutoTuneSweep() override {
116 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
117 0 : }
118 :
119 : void Log_Write_AutoTune(AxisType axis, TuneType tune_step,
120 : float meas_target, float meas_min, float meas_max,
121 : float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
122 : void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
123 : #endif
124 :
125 : // Defines the sequence of tuning steps (P, D, etc.)
126 19 : void set_tune_sequence() override {
127 19 : tune_seq[0] = TuneType::RATE_D_UP;
128 19 : tune_seq[1] = TuneType::RATE_D_DOWN;
129 19 : tune_seq[2] = TuneType::RATE_P_UP;
130 19 : tune_seq[3] = TuneType::ANGLE_P_DOWN;
131 19 : tune_seq[4] = TuneType::ANGLE_P_UP;
132 19 : tune_seq[5] = TuneType::TUNE_COMPLETE;
133 19 : }
134 :
135 : // Axis tuning mask accessor
136 6654 : uint8_t get_axis_bitmask() const override { return axis_bitmask; }
137 :
138 : // Timeout for a single twitch test
139 : uint32_t get_testing_step_timeout_ms() const override;
140 :
141 : private:
142 : // Helpers for twitch-based test monitoring
143 :
144 : void twitching_test_rate(float angle, float rate, float rate_target,
145 : float &meas_rate_min, float &meas_rate_max, float &meas_angle_min);
146 :
147 : void twitching_abort_rate(float angle, float rate, float angle_max,
148 : float meas_rate_min, float angle_min);
149 :
150 : void twitching_test_angle(float angle, float rate, float angle_target,
151 : float &meas_angle_min, float &meas_angle_max,
152 : float &meas_rate_min, float &meas_rate_max);
153 :
154 : // Calculates average acceleration during twitch
155 : void twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const;
156 :
157 : // Gain update routines
158 :
159 : // Increases D and adjusts P to produce controlled bounce-back behavior.
160 : // Seeks to bring the response peak just below the target rate while inducing
161 : // a small overshoot (bounce) for evaluating D influence.
162 : void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio,
163 : float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio,
164 : float rate_target, float meas_rate_min, float meas_rate_max);
165 :
166 : // Decreases D and adjusts P to eliminate bounce-back in the response.
167 : // Aims to achieve a clean response just below the target rate with no overshoot.
168 : void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio,
169 : float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio,
170 : float rate_target, float meas_rate_min, float meas_rate_max);
171 :
172 : // Increases P to ensure the target rate is reached quickly, while reducing D
173 : // if bounce-back exceeds the aggressiveness threshold.
174 : // Fails if D hits minimum and clean response is not achievable (optional).
175 : void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio,
176 : float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio,
177 : float rate_target, float meas_rate_min, float meas_rate_max,
178 : bool fail_min_d = true);
179 :
180 : // Decreases angle P gain to reduce overshoot until the target is no longer reached before timeout.
181 : // Used to back off from aggressive behavior in angle control.
182 : void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio,
183 : float angle_target, float meas_angle_max,
184 : float meas_rate_min, float meas_rate_max);
185 :
186 : // Increases angle P gain to ensure the target is reached within a reasonable time.
187 : // Stops increasing once target response is consistently achieved without overshoot.
188 : void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio,
189 : float angle_target, float meas_angle_max,
190 : float meas_rate_min, float meas_rate_max);
191 :
192 : // Formats and sends gain reports
193 : void report_axis_gains(const char* axis_string, float rate_P, float rate_I,
194 : float rate_D, float angle_P, float max_accel_radss) const;
195 :
196 : // Parameters
197 : AP_Int8 axis_bitmask; // Axis enable mask
198 : AP_Float aggressiveness; // Target overshoot ratio (D tuning sensitivity)
199 : AP_Float min_d; // Minimum allowed D gain
200 : AP_Float gain_backoff; // Fraction by which tuned P and D gains are reduced after each AutoTune stage (rate loop, then angle loop) to provide additional stability margin
201 : bool ignore_next; // Skip next result (used for rate overshoot handling)
202 :
203 : // Measurement and target values for each test step
204 : float target_angle;
205 : float target_rate;
206 : float angle_abort;
207 :
208 : float test_rate_min;
209 : float test_rate_max;
210 : float test_angle_min;
211 : float test_angle_max;
212 :
213 : float accel_measure_rate_max;
214 : };
215 :
216 : #endif // AC_AUTOTUNE_ENABLED
|
