Line data Source code
1 : #include <AP_Tuning/AP_Tuning_config.h>
2 :
3 : #if AP_TUNING_ENABLED
4 :
5 : #include "Plane.h"
6 :
7 : /*
8 : the vehicle class has its own var table for TUNE_PARAM so it can
9 : have separate parameter docs for the list of available parameters
10 : */
11 : const AP_Param::GroupInfo AP_Tuning_Plane::var_info[] = {
12 : // @Param: PARAM
13 : // @DisplayName: Transmitter tuning parameter or set of parameters
14 : // @Description: This sets which parameter or set of parameters will be tuned. Values greater than 100 indicate a set of parameters rather than a single parameter. Parameters less than 50 are for QuadPlane vertical lift motors only.
15 : // @Values: 0:None,1:RateRollPI,2:RateRollP,3:RateRollI,4:RateRollD,5:RatePitchPI,6:RatePitchP,7:RatePitchI,8:RatePitchD,9:RateYawPI,10:RateYawP,11:RateYawI,12:RateYawD,13:AngleRollP,14:AnglePitchP,15:AngleYawP,16:PosXYP,17:PosZP,18:VelXYP,19:VelXYI,20:VelZP,21:AccelZP,22:AccelZI,23:AccelZD,24:RatePitchFF,25:RateRollFF,26:RateYawFF,50:FixedWingRollP,51:FixedWingRollI,52:FixedWingRollD,53:FixedWingRollFF,54:FixedWingPitchP,55:FixedWingPitchI,56:FixedWingPitchD,57:FixedWingPitchFF,101:Set_RateRollPitch,102:Set_RateRoll,103:Set_RatePitch,104:Set_RateYaw,105:Set_AngleRollPitch,106:Set_VelXY,107:Set_AccelZ,108:Set_RatePitchDP,109:Set_RateRollDP,110:Set_RateYawDP
16 : // @User: Standard
17 : AP_GROUPINFO("PARAM", 1, AP_Tuning_Plane, parmset, 0),
18 :
19 : // the rest of the parameters are from AP_Tuning
20 : AP_NESTEDGROUPINFO(AP_Tuning, 0),
21 :
22 : AP_GROUPEND
23 : };
24 :
25 :
26 : /*
27 : tables of tuning sets
28 : */
29 : const uint8_t AP_Tuning_Plane::tuning_set_rate_roll_pitch[] = { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_PI,
30 : TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_PI};
31 : const uint8_t AP_Tuning_Plane::tuning_set_rate_roll[] = { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_PI };
32 : const uint8_t AP_Tuning_Plane::tuning_set_rate_pitch[] = { TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_PI };
33 : const uint8_t AP_Tuning_Plane::tuning_set_rate_yaw[] = { TUNING_RATE_YAW_P, TUNING_RATE_YAW_I, TUNING_RATE_YAW_D };
34 : const uint8_t AP_Tuning_Plane::tuning_set_ang_roll_pitch[] = { TUNING_ANG_ROLL_P, TUNING_ANG_PITCH_P };
35 : const uint8_t AP_Tuning_Plane::tuning_set_vxy[] = { TUNING_VXY_P, TUNING_VXY_I };
36 : const uint8_t AP_Tuning_Plane::tuning_set_az[] = { TUNING_AZ_P, TUNING_AZ_I, TUNING_AZ_D };
37 : const uint8_t AP_Tuning_Plane::tuning_set_rate_pitchDP[]= { TUNING_RATE_PITCH_D, TUNING_RATE_PITCH_P };
38 : const uint8_t AP_Tuning_Plane::tuning_set_rate_rollDP[]= { TUNING_RATE_ROLL_D, TUNING_RATE_ROLL_P };
39 : const uint8_t AP_Tuning_Plane::tuning_set_rate_yawDP[]= { TUNING_RATE_YAW_D, TUNING_RATE_YAW_P };
40 : const uint8_t AP_Tuning_Plane::tuning_set_dp_roll_pitch[] = { TUNING_RLL_D, TUNING_RLL_P, TUNING_PIT_D, TUNING_PIT_P };
41 : const uint8_t AP_Tuning_Plane::tuning_set_pidff_roll[] = { TUNING_RLL_P, TUNING_RLL_I, TUNING_RLL_D, TUNING_RLL_FF };
42 : const uint8_t AP_Tuning_Plane::tuning_set_pidff_pitch[] = { TUNING_PIT_P, TUNING_PIT_I, TUNING_PIT_D, TUNING_PIT_FF };
43 :
44 : // macro to prevent getting the array length wrong
45 : #define TUNING_ARRAY(v) ARRAY_SIZE(v), v
46 :
47 : // list of tuning sets
48 : const AP_Tuning_Plane::tuning_set AP_Tuning_Plane::tuning_sets[] = {
49 : { TUNING_SET_RATE_ROLL_PITCH, TUNING_ARRAY(tuning_set_rate_roll_pitch) },
50 : { TUNING_SET_RATE_ROLL, TUNING_ARRAY(tuning_set_rate_roll) },
51 : { TUNING_SET_RATE_PITCH, TUNING_ARRAY(tuning_set_rate_pitch) },
52 : { TUNING_SET_RATE_YAW, TUNING_ARRAY(tuning_set_rate_yaw) },
53 : { TUNING_SET_ANG_ROLL_PITCH, TUNING_ARRAY(tuning_set_ang_roll_pitch) },
54 : { TUNING_SET_VXY, TUNING_ARRAY(tuning_set_vxy) },
55 : { TUNING_SET_AZ, TUNING_ARRAY(tuning_set_az) },
56 : { TUNING_SET_RATE_PITCHDP, TUNING_ARRAY(tuning_set_rate_pitchDP) },
57 : { TUNING_SET_RATE_ROLLDP, TUNING_ARRAY(tuning_set_rate_rollDP) },
58 : { TUNING_SET_RATE_YAWDP, TUNING_ARRAY(tuning_set_rate_yawDP) },
59 : { TUNING_SET_DP_ROLL_PITCH, TUNING_ARRAY(tuning_set_dp_roll_pitch) },
60 : { TUNING_SET_PIDFF_ROLL, TUNING_ARRAY(tuning_set_pidff_roll) },
61 : { TUNING_SET_PIDFF_PITCH, TUNING_ARRAY(tuning_set_pidff_pitch) },
62 : { 0, 0, nullptr }
63 : };
64 :
65 : /*
66 : table of tuning names
67 : */
68 : const AP_Tuning_Plane::tuning_name AP_Tuning_Plane::tuning_names[] = {
69 : { TUNING_RATE_ROLL_PI, "RateRollPI" },
70 : { TUNING_RATE_ROLL_P, "RateRollP" },
71 : { TUNING_RATE_ROLL_I, "RateRollI" },
72 : { TUNING_RATE_ROLL_D, "RateRollD" },
73 : { TUNING_RATE_PITCH_PI,"RatePitchPI" },
74 : { TUNING_RATE_PITCH_P, "RatePitchP" },
75 : { TUNING_RATE_PITCH_I, "RatePitchI" },
76 : { TUNING_RATE_PITCH_D, "RatePitchD" },
77 : { TUNING_RATE_YAW_PI, "RateYawPI" },
78 : { TUNING_RATE_YAW_P, "RateYawP" },
79 : { TUNING_RATE_YAW_I, "RateYawI" },
80 : { TUNING_RATE_YAW_D, "RateYawD" },
81 : { TUNING_ANG_ROLL_P, "AngRollP" },
82 : { TUNING_ANG_PITCH_P, "AngPitchP" },
83 : { TUNING_ANG_YAW_P, "AngYawP" },
84 : { TUNING_RATE_PITCH_FF, "RatePitchFF" },
85 : { TUNING_RATE_ROLL_FF, "RateRollFF" },
86 : { TUNING_RATE_YAW_FF, "RateYawFF" },
87 : { TUNING_PXY_P, "PXY_P" },
88 : { TUNING_PZ_P, "PZ_P" },
89 : { TUNING_VXY_P, "VXY_P" },
90 : { TUNING_VXY_I, "VXY_I" },
91 : { TUNING_VZ_P, "VZ_P" },
92 : { TUNING_AZ_P, "RateAZ_P" },
93 : { TUNING_AZ_I, "RateAZ_I" },
94 : { TUNING_AZ_D, "RateAZ_D" },
95 : { TUNING_RLL_P, "RollP" },
96 : { TUNING_RLL_I, "RollI" },
97 : { TUNING_RLL_D, "RollD" },
98 : { TUNING_RLL_FF, "RollFF" },
99 : { TUNING_PIT_P, "PitchP" },
100 : { TUNING_PIT_I, "PitchI" },
101 : { TUNING_PIT_D, "PitchD" },
102 : { TUNING_PIT_FF, "PitchFF" },
103 : { TUNING_Q_FWD_THR, "QModeFwdThr" },
104 : { TUNING_NONE, nullptr }
105 : };
106 :
107 : /*
108 : get a pointer to an AP_Float for a parameter, or nullptr on fail
109 : */
110 0 : AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm)
111 : {
112 : #if HAL_QUADPLANE_ENABLED
113 0 : if (parm < TUNING_FIXED_WING_BASE && !plane.quadplane.available()) {
114 : // quadplane tuning options not available
115 0 : return nullptr;
116 : }
117 : #endif
118 :
119 0 : switch(parm) {
120 :
121 : #if HAL_QUADPLANE_ENABLED
122 0 : case TUNING_RATE_ROLL_PI:
123 : // use P for initial value when tuning PI
124 0 : return &plane.quadplane.attitude_control->get_rate_roll_pid().kP();
125 :
126 0 : case TUNING_RATE_ROLL_P:
127 0 : return &plane.quadplane.attitude_control->get_rate_roll_pid().kP();
128 :
129 0 : case TUNING_RATE_ROLL_I:
130 0 : return &plane.quadplane.attitude_control->get_rate_roll_pid().kI();
131 :
132 0 : case TUNING_RATE_ROLL_D:
133 0 : return &plane.quadplane.attitude_control->get_rate_roll_pid().kD();
134 :
135 0 : case TUNING_RATE_PITCH_PI:
136 0 : return &plane.quadplane.attitude_control->get_rate_pitch_pid().kP();
137 :
138 0 : case TUNING_RATE_PITCH_P:
139 0 : return &plane.quadplane.attitude_control->get_rate_pitch_pid().kP();
140 :
141 0 : case TUNING_RATE_PITCH_I:
142 0 : return &plane.quadplane.attitude_control->get_rate_pitch_pid().kI();
143 :
144 0 : case TUNING_RATE_PITCH_D:
145 0 : return &plane.quadplane.attitude_control->get_rate_pitch_pid().kD();
146 :
147 0 : case TUNING_RATE_YAW_PI:
148 0 : return &plane.quadplane.attitude_control->get_rate_yaw_pid().kP();
149 :
150 0 : case TUNING_RATE_YAW_P:
151 0 : return &plane.quadplane.attitude_control->get_rate_yaw_pid().kP();
152 :
153 0 : case TUNING_RATE_YAW_I:
154 0 : return &plane.quadplane.attitude_control->get_rate_yaw_pid().kI();
155 :
156 0 : case TUNING_RATE_YAW_D:
157 0 : return &plane.quadplane.attitude_control->get_rate_yaw_pid().kD();
158 :
159 0 : case TUNING_ANG_ROLL_P:
160 0 : return &plane.quadplane.attitude_control->get_angle_roll_p().kP();
161 :
162 0 : case TUNING_ANG_PITCH_P:
163 0 : return &plane.quadplane.attitude_control->get_angle_pitch_p().kP();
164 :
165 0 : case TUNING_ANG_YAW_P:
166 0 : return &plane.quadplane.attitude_control->get_angle_yaw_p().kP();
167 :
168 0 : case TUNING_PXY_P:
169 0 : return &plane.quadplane.pos_control->NE_get_pos_p().kP();
170 :
171 0 : case TUNING_PZ_P:
172 0 : return &plane.quadplane.pos_control->D_get_pos_p().kP();
173 :
174 0 : case TUNING_VXY_P:
175 0 : return &plane.quadplane.pos_control->NE_get_vel_pid().kP();
176 :
177 0 : case TUNING_VXY_I:
178 0 : return &plane.quadplane.pos_control->NE_get_vel_pid().kI();
179 :
180 0 : case TUNING_VZ_P:
181 0 : return &plane.quadplane.pos_control->D_get_vel_pid().kP();
182 :
183 0 : case TUNING_AZ_P:
184 0 : return &plane.quadplane.pos_control->D_get_accel_pid().kP();
185 :
186 0 : case TUNING_AZ_I:
187 0 : return &plane.quadplane.pos_control->D_get_accel_pid().kI();
188 :
189 0 : case TUNING_AZ_D:
190 0 : return &plane.quadplane.pos_control->D_get_accel_pid().kD();
191 :
192 0 : case TUNING_RATE_PITCH_FF:
193 0 : return &plane.quadplane.attitude_control->get_rate_pitch_pid().ff();
194 :
195 0 : case TUNING_RATE_ROLL_FF:
196 0 : return &plane.quadplane.attitude_control->get_rate_roll_pid().ff();
197 :
198 0 : case TUNING_RATE_YAW_FF:
199 0 : return &plane.quadplane.attitude_control->get_rate_yaw_pid().ff();
200 :
201 0 : case TUNING_Q_FWD_THR:
202 0 : return &plane.quadplane.q_fwd_thr_gain;
203 : #endif // HAL_QUADPLANE_ENABLED
204 :
205 : // fixed wing tuning parameters
206 0 : case TUNING_RLL_P:
207 0 : return &plane.rollController.kP();
208 :
209 0 : case TUNING_RLL_I:
210 0 : return &plane.rollController.kI();
211 :
212 0 : case TUNING_RLL_D:
213 0 : return &plane.rollController.kD();
214 :
215 0 : case TUNING_RLL_FF:
216 0 : return &plane.rollController.kFF();
217 :
218 0 : case TUNING_PIT_P:
219 0 : return &plane.pitchController.kP();
220 :
221 0 : case TUNING_PIT_I:
222 0 : return &plane.pitchController.kI();
223 :
224 0 : case TUNING_PIT_D:
225 0 : return &plane.pitchController.kD();
226 :
227 0 : case TUNING_PIT_FF:
228 0 : return &plane.pitchController.kFF();
229 : }
230 0 : return nullptr;
231 : }
232 :
233 :
234 : /*
235 : save a parameter
236 : */
237 0 : void AP_Tuning_Plane::save_value(uint8_t parm)
238 : {
239 0 : switch(parm) {
240 : // special handling of dual-parameters
241 0 : case TUNING_RATE_ROLL_PI:
242 0 : save_value(TUNING_RATE_ROLL_P);
243 0 : save_value(TUNING_RATE_ROLL_I);
244 0 : break;
245 0 : case TUNING_RATE_PITCH_PI:
246 0 : save_value(TUNING_RATE_PITCH_P);
247 0 : save_value(TUNING_RATE_PITCH_I);
248 0 : break;
249 0 : default:
250 0 : AP_Float *f = get_param_pointer(parm);
251 0 : if (f != nullptr) {
252 0 : f->save();
253 : }
254 0 : break;
255 : }
256 0 : }
257 :
258 : /*
259 : set a parameter
260 : */
261 0 : void AP_Tuning_Plane::set_value(uint8_t parm, float value)
262 : {
263 0 : switch(parm) {
264 : // special handling of dual-parameters
265 0 : case TUNING_RATE_ROLL_PI:
266 0 : set_value(TUNING_RATE_ROLL_P, value);
267 0 : set_value(TUNING_RATE_ROLL_I, value);
268 0 : break;
269 0 : case TUNING_RATE_PITCH_PI:
270 0 : set_value(TUNING_RATE_PITCH_P, value);
271 0 : set_value(TUNING_RATE_PITCH_I, value);
272 0 : break;
273 0 : default:
274 0 : AP_Float *f = get_param_pointer(parm);
275 0 : if (f != nullptr) {
276 0 : uint64_t param_bit = (1ULL << parm);
277 0 : if (!(param_bit & have_set)) {
278 : // first time this param has been set by tuning. We
279 : // need to see if a reversion value is available in
280 : // FRAM, and if not then save one
281 0 : float current_value = f->get();
282 0 : if (!f->load()) {
283 : // there is no value in FRAM, set one
284 0 : f->set_and_save(current_value);
285 : }
286 0 : have_set |= param_bit;
287 : }
288 0 : f->set_and_notify(value);
289 : }
290 0 : break;
291 : }
292 0 : }
293 :
294 : /*
295 : reload a parameter
296 : */
297 0 : void AP_Tuning_Plane::reload_value(uint8_t parm)
298 : {
299 0 : switch(parm) {
300 : // special handling of dual-parameters
301 0 : case TUNING_RATE_ROLL_PI:
302 0 : reload_value(TUNING_RATE_ROLL_P);
303 0 : reload_value(TUNING_RATE_ROLL_I);
304 0 : break;
305 0 : case TUNING_RATE_PITCH_PI:
306 0 : reload_value(TUNING_RATE_PITCH_P);
307 0 : reload_value(TUNING_RATE_PITCH_I);
308 0 : break;
309 0 : default:
310 0 : AP_Float *f = get_param_pointer(parm);
311 0 : if (f != nullptr) {
312 0 : uint64_t param_bit = (1ULL << parm);
313 : // only reload if we have set this parameter at some point
314 0 : if (param_bit & have_set) {
315 0 : f->load();
316 : }
317 : }
318 0 : break;
319 : }
320 0 : }
321 :
322 : #endif // AP_TUNING_ENABLED
|
