Line data Source code
1 : #include "AP_Tuning_config.h"
2 :
3 : #if AP_TUNING_ENABLED
4 :
5 : #include "AP_Tuning.h"
6 :
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <GCS_MAVLink/GCS.h>
9 : #include <RC_Channel/RC_Channel.h>
10 : #include <AP_Notify/AP_Notify.h>
11 :
12 : extern const AP_HAL::HAL& hal;
13 :
14 : const AP_Param::GroupInfo AP_Tuning::var_info[] = {
15 : // @Param: CHAN
16 : // @DisplayName: Transmitter tuning channel
17 : // @Description: This sets the channel for transmitter tuning. This should be connected to a knob or slider on your transmitter. It needs to be setup to use the PWM range given by TUNE_CHAN_MIN to TUNE_CHAN_MAX
18 : // @Values: 0:Disable,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
19 : // @User: Standard
20 : AP_GROUPINFO("CHAN", 1, AP_Tuning, channel, 0),
21 :
22 : // @Param: CHAN_MIN
23 : // @DisplayName: Transmitter tuning channel minimum pwm
24 : // @Description: This sets the PWM lower limit for the tuning channel
25 : // @Range: 900 2100
26 : // @User: Standard
27 : AP_GROUPINFO("CHAN_MIN", 2, AP_Tuning, channel_min, 1000),
28 :
29 : // @Param: CHAN_MAX
30 : // @DisplayName: Transmitter tuning channel maximum pwm
31 : // @Description: This sets the PWM upper limit for the tuning channel
32 : // @Range: 900 2100
33 : // @User: Standard
34 : AP_GROUPINFO("CHAN_MAX", 3, AP_Tuning, channel_max, 2000),
35 :
36 : // @Param: SELECTOR
37 : // @DisplayName: Transmitter tuning selector channel
38 : // @Description: This sets the channel for the transmitter tuning selector switch. This should be a 2 position switch, preferably spring loaded. A PWM above 1700 means high, below 1300 means low. If no selector is set then you won't be able to switch between parameters during flight or re-center the tuning knob
39 : // @Values: 0:Disable,1:Chan1,2:Chan3,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
40 : // @User: Standard
41 : AP_GROUPINFO("SELECTOR", 4, AP_Tuning, selector, 0),
42 :
43 : // @Param: RANGE
44 : // @DisplayName: Transmitter tuning range
45 : // @Description: This sets the range over which tuning will change a parameter. A value of 2 means the tuning parameter will go from 0.5 times the start value to 2x the start value over the range of the tuning channel
46 : // @User: Standard
47 : AP_GROUPINFO("RANGE", 5, AP_Tuning, range, 2.0f),
48 :
49 : // @Param: MODE_REVERT
50 : // @DisplayName: Revert on mode change
51 : // @Description: This controls whether tuning values will revert on a flight mode change.
52 : // @Values: 0:Disable,1:Enable
53 : // @User: Standard
54 : AP_GROUPINFO("MODE_REVERT", 6, AP_Tuning, mode_revert, 1),
55 :
56 : // @Param: ERR_THRESH
57 : // @DisplayName: Controller error threshold
58 : // @Description: This sets the controller error threshold above which an alarm will sound and a message will be sent to the GCS to warn of controller instability while tuning. The error is the rms value of the P+D corrections in the loop. High values in hover indicate possible instability due to too high PID gains or excessively high D to P gain ratios.-1 will disable this message.
59 : // @Range: 0 1
60 : // @User: Standard
61 : AP_GROUPINFO("ERR_THRESH", 7, AP_Tuning, error_threshold, 0.15f),
62 :
63 : AP_GROUPEND
64 : };
65 :
66 : /*
67 : handle selector switch input
68 : */
69 0 : void AP_Tuning::check_selector_switch(void)
70 : {
71 0 : if (selector == 0) {
72 : // no selector switch enabled
73 0 : return;
74 : }
75 0 : if (!rc().has_valid_input()) {
76 0 : selector_start_ms = 0;
77 0 : return;
78 : }
79 0 : RC_Channel *selchan = rc().channel(selector-1);
80 0 : if (selchan == nullptr) {
81 0 : return;
82 : }
83 0 : uint16_t selector_in = selchan->get_radio_in();
84 0 : if (selector_in >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
85 : // high selector
86 0 : if (selector_start_ms == 0) {
87 0 : selector_start_ms = AP_HAL::millis();
88 : }
89 0 : uint32_t hold_time = AP_HAL::millis() - selector_start_ms;
90 0 : if (hold_time > 5000 && changed) {
91 : // save tune
92 0 : save_parameters();
93 0 : re_center();
94 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: Saved");
95 0 : AP_Notify::events.tune_save = 1;
96 0 : changed = false;
97 0 : need_revert = 0;
98 : }
99 0 : } else if (selector_in <= RC_Channel::AUX_PWM_TRIGGER_LOW) {
100 : // low selector
101 0 : if (selector_start_ms != 0) {
102 0 : uint32_t hold_time = AP_HAL::millis() - selector_start_ms;
103 0 : if (hold_time < 200) {
104 : // debounce!
105 0 : } else if (hold_time < 2000) {
106 : // re-center the value
107 0 : re_center();
108 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: recentered %s", get_tuning_name(current_parm));
109 0 : } else if (hold_time < 5000) {
110 : // change parameter
111 0 : next_parameter();
112 : }
113 0 : selector_start_ms = 0;
114 : }
115 : }
116 : }
117 :
118 : /*
119 : re-center the tuning value
120 : */
121 0 : void AP_Tuning::re_center(void)
122 : {
123 0 : AP_Float *f = get_param_pointer(current_parm);
124 0 : if (f != nullptr) {
125 0 : center_value = f->get();
126 0 : old_value = 0.0;
127 : }
128 0 : mid_point_wait = true;
129 0 : }
130 :
131 : /*
132 : check for changed tuning input
133 : */
134 2710433 : void AP_Tuning::check_input(uint8_t flightmode)
135 : {
136 2710433 : if (channel <= 0 || parmset <= 0) {
137 : // disabled
138 2710433 : return;
139 : }
140 :
141 : // check for revert on changed flightmode
142 0 : if (flightmode != last_flightmode) {
143 0 : if (need_revert != 0 && mode_revert != 0) {
144 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: reverted");
145 0 : revert_parameters();
146 0 : re_center();
147 : }
148 0 : last_flightmode = flightmode;
149 : }
150 :
151 : // only adjust values at 10Hz
152 0 : uint32_t now = AP_HAL::millis();
153 0 : uint32_t dt_ms = now - last_check_ms;
154 0 : if (dt_ms < 100) {
155 0 : return;
156 : }
157 0 : last_check_ms = now;
158 :
159 0 : if (channel > RC_Channels::get_valid_channel_count()) {
160 : // not valid channel
161 0 : return;
162 : }
163 :
164 : // check for invalid range
165 0 : if (range < 1.1f) {
166 0 : range.set(1.1f);
167 : }
168 :
169 0 : if (current_parm == 0) {
170 0 : next_parameter();
171 : }
172 :
173 : // cope with user changing parmset while tuning
174 0 : if (current_set != parmset) {
175 0 : re_center();
176 : }
177 0 : current_set = parmset;
178 :
179 0 : check_selector_switch();
180 :
181 0 : if (selector_start_ms) {
182 : // no tuning while selector high
183 0 : return;
184 : }
185 :
186 0 : if (current_parm == 0) {
187 0 : return;
188 : }
189 :
190 0 : RC_Channel *chan = rc().channel(channel-1);
191 0 : if (chan == nullptr) {
192 0 : return;
193 : }
194 0 : float chan_value = linear_interpolate(-1, 1, chan->get_radio_in(), channel_min, channel_max);
195 0 : if (dt_ms > 500) {
196 0 : last_channel_value = chan_value;
197 : }
198 :
199 0 : if (fabsf(chan_value - last_channel_value) < 0.01) {
200 : // ignore changes of less than 1%
201 0 : return;
202 : }
203 :
204 : //hal.console->printf("chan_value %.2f last_channel_value %.2f\n", chan_value, last_channel_value);
205 :
206 0 : const float dead_zone = 0.02;
207 0 : if (mid_point_wait) {
208 : // see if we have crossed the mid-point. We use a small deadzone to make it easier
209 : // to move to the "indent" portion of a slider to start tuning
210 0 : if ((chan_value > dead_zone && last_channel_value > 0) ||
211 0 : (chan_value < -dead_zone && last_channel_value < 0)) {
212 : // still waiting
213 0 : return;
214 : }
215 : // starting tuning
216 0 : mid_point_wait = false;
217 0 : AP_Notify::events.tune_started = 1;
218 : }
219 0 : last_channel_value = chan_value;
220 :
221 : float new_value;
222 0 : if (chan_value > 0) {
223 0 : new_value = linear_interpolate(center_value, range*center_value, chan_value, 0, 1);
224 : } else {
225 0 : new_value = linear_interpolate(center_value/range, center_value, chan_value, -1, 0);
226 : }
227 0 : changed = true;
228 0 : need_revert |= (1U << current_parm_index);
229 0 : set_value(current_parm, new_value);
230 :
231 0 : if ( fabsf(new_value-old_value) > (0.05 * old_value) ) {
232 0 : old_value = new_value;
233 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
234 : "Tuning: %s%s%0.4f",
235 : get_tuning_name(current_parm),
236 : ((chan_value < dead_zone) && (chan_value > -dead_zone)) ? "> " : ": ",
237 : (double)(new_value));
238 : }
239 :
240 : #if HAL_LOGGING_ENABLED
241 0 : Log_Write_Parameter_Tuning(new_value);
242 : #endif
243 : }
244 :
245 :
246 : #if HAL_LOGGING_ENABLED
247 : /*
248 : log a tuning change
249 : */
250 0 : void AP_Tuning::Log_Write_Parameter_Tuning(float value)
251 : {
252 : // @LoggerMessage: PRTN
253 : // @Description: Plane Parameter Tuning data
254 : // @Field: TimeUS: Time since system startup
255 : // @Field: Set: Parameter set being tuned
256 : // @Field: Parm: Parameter being tuned
257 : // @Field: Value: Current parameter value
258 : // @Field: CenterValue: Center value (startpoint of current modifications) of parameter being tuned
259 0 : AP::logger().Write("PRTN", "TimeUS,Set,Parm,Value,CenterValue", "QBBff",
260 : AP_HAL::micros64(),
261 : parmset,
262 0 : current_parm,
263 : (double)value,
264 0 : (double)center_value);
265 0 : }
266 : #endif
267 :
268 : /*
269 : save parameters in the set
270 : */
271 0 : void AP_Tuning::save_parameters(void)
272 : {
273 0 : uint8_t set = (uint8_t)parmset.get();
274 0 : if (set < set_base) {
275 : // single parameter tuning
276 0 : save_value(set);
277 0 : return;
278 : }
279 : // multiple parameter tuning
280 0 : for (uint8_t i=0; tuning_sets[i].num_parms != 0; i++) {
281 0 : if (tuning_sets[i].set+set_base == set) {
282 0 : for (uint8_t p=0; p<tuning_sets[i].num_parms; p++) {
283 0 : save_value(tuning_sets[i].parms[p]);
284 : }
285 0 : break;
286 : }
287 : }
288 : }
289 :
290 :
291 : /*
292 : save parameters in the set
293 : */
294 0 : void AP_Tuning::revert_parameters(void)
295 : {
296 0 : uint8_t set = (uint8_t)parmset.get();
297 0 : if (set < set_base) {
298 : // single parameter tuning
299 0 : reload_value(set);
300 0 : return;
301 : }
302 0 : for (uint8_t i=0; tuning_sets[i].num_parms != 0; i++) {
303 0 : if (tuning_sets[i].set+set_base == set) {
304 0 : for (uint8_t p=0; p<tuning_sets[i].num_parms; p++) {
305 0 : if (p >= 32 || (need_revert & (1U<<p))) {
306 0 : reload_value(tuning_sets[i].parms[p]);
307 : }
308 : }
309 0 : need_revert = 0;
310 0 : break;
311 : }
312 : }
313 : }
314 :
315 : /*
316 : switch to the next parameter in the set
317 : */
318 0 : void AP_Tuning::next_parameter(void)
319 : {
320 0 : uint8_t set = (uint8_t)parmset.get();
321 0 : if (set < set_base) {
322 : // nothing to do but re-center
323 0 : current_parm = set;
324 0 : re_center();
325 0 : return;
326 : }
327 0 : for (uint8_t i=0; tuning_sets[i].num_parms != 0; i++) {
328 0 : if (tuning_sets[i].set+set_base == set) {
329 0 : if (current_parm == 0) {
330 0 : current_parm_index = 0;
331 : } else {
332 0 : current_parm_index = (current_parm_index + 1) % tuning_sets[i].num_parms;
333 : }
334 0 : current_parm = tuning_sets[i].parms[current_parm_index];
335 0 : re_center();
336 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Tuning: started %s", get_tuning_name(current_parm));
337 0 : AP_Notify::events.tune_next = current_parm_index+1;
338 0 : break;
339 : }
340 : }
341 : }
342 :
343 : /*
344 : return a string representing a tuning parameter
345 : */
346 0 : const char *AP_Tuning::get_tuning_name(uint8_t parm)
347 : {
348 0 : for (uint8_t i=0; tuning_names[i].name != nullptr; i++) {
349 0 : if (parm == tuning_names[i].parm) {
350 0 : return tuning_names[i].name;
351 : }
352 : }
353 0 : return "UNKNOWN";
354 : }
355 :
356 : #endif // AP_TUNING_ENABLED
|
