Merge pull request #11270 from haslinghuis/rename_attr
[betaflight.git] / src / main / fc / rc_adjustments.c
blobad4cbf052980a48117f9d5f4483eddc4bd4d04ff
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
25 #include <math.h>
27 #include "platform.h"
29 #include "blackbox/blackbox.h"
30 #include "blackbox/blackbox_fielddefs.h"
32 #include "build/build_config.h"
34 #include "common/axis.h"
35 #include "common/maths.h"
36 #include "common/utils.h"
38 #include "config/feature.h"
40 #include "drivers/time.h"
42 #include "config/config.h"
43 #include "fc/controlrate_profile.h"
44 #include "fc/rc_controls.h"
45 #include "fc/rc.h"
47 #include "flight/pid.h"
48 #include "flight/pid_init.h"
50 #include "io/beeper.h"
51 #include "io/ledstrip.h"
52 #include "io/pidaudio.h"
54 #include "osd/osd.h"
56 #include "pg/pg.h"
57 #include "pg/pg_ids.h"
58 #include "pg/rx.h"
60 #include "rx/rx.h"
62 #include "rc_adjustments.h"
64 #include "scheduler/scheduler.h"
66 #define ADJUSTMENT_RANGE_COUNT_INVALID -1
68 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 2);
70 uint8_t pidAudioPositionToModeMap[7] = {
71 // on a pot with a center detent, it's easy to have center area for off/default, then three positions to the left and three to the right.
72 // current implementation yields RC values as below.
74 PID_AUDIO_PIDSUM_X, // 900 - ~1071 - Min
75 PID_AUDIO_PIDSUM_Y, // ~1071 - ~1242
76 PID_AUDIO_PIDSUM_XY, // ~1242 - ~1414
77 PID_AUDIO_OFF, // ~1414 - ~1585 - Center
78 PID_AUDIO_OFF, // ~1585 - ~1757
79 PID_AUDIO_OFF, // ~1757 - ~1928
80 PID_AUDIO_OFF, // ~1928 - 2100 - Max
82 // Note: Last 3 positions are currently pending implementations and use PID_AUDIO_OFF for now.
85 STATIC_UNIT_TESTED int stepwiseAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;
86 STATIC_UNIT_TESTED timedAdjustmentState_t stepwiseAdjustments[MAX_ADJUSTMENT_RANGE_COUNT];
88 STATIC_UNIT_TESTED int continuosAdjustmentCount;
89 STATIC_UNIT_TESTED continuosAdjustmentState_t continuosAdjustments[MAX_ADJUSTMENT_RANGE_COUNT];
91 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
93 #ifndef USE_BLACKBOX
94 UNUSED(adjustmentFunction);
95 UNUSED(newValue);
96 #else
97 if (blackboxConfig()->device) {
98 flightLogEvent_inflightAdjustment_t eventData;
99 eventData.adjustmentFunction = adjustmentFunction;
100 eventData.newValue = newValue;
101 eventData.floatFlag = false;
102 blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
104 #endif
107 // sync with adjustmentFunction_e
108 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
110 .adjustmentFunction = ADJUSTMENT_RC_RATE,
111 .mode = ADJUSTMENT_MODE_STEP,
112 .data = { .step = 1 }
113 }, {
114 .adjustmentFunction = ADJUSTMENT_RC_EXPO,
115 .mode = ADJUSTMENT_MODE_STEP,
116 .data = { .step = 1 }
117 }, {
118 .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
119 .mode = ADJUSTMENT_MODE_STEP,
120 .data = { .step = 1 }
121 }, {
122 .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
123 .mode = ADJUSTMENT_MODE_STEP,
124 .data = { .step = 1 }
125 }, {
126 .adjustmentFunction = ADJUSTMENT_YAW_RATE,
127 .mode = ADJUSTMENT_MODE_STEP,
128 .data = { .step = 1 }
129 }, {
130 .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
131 .mode = ADJUSTMENT_MODE_STEP,
132 .data = { .step = 1 }
133 }, {
134 .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
135 .mode = ADJUSTMENT_MODE_STEP,
136 .data = { .step = 1 }
137 }, {
138 .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
139 .mode = ADJUSTMENT_MODE_STEP,
140 .data = { .step = 1 }
141 }, {
142 .adjustmentFunction = ADJUSTMENT_YAW_P,
143 .mode = ADJUSTMENT_MODE_STEP,
144 .data = { .step = 1 }
145 }, {
146 .adjustmentFunction = ADJUSTMENT_YAW_I,
147 .mode = ADJUSTMENT_MODE_STEP,
148 .data = { .step = 1 }
149 }, {
150 .adjustmentFunction = ADJUSTMENT_YAW_D,
151 .mode = ADJUSTMENT_MODE_STEP,
152 .data = { .step = 1 }
153 }, {
154 .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
155 .mode = ADJUSTMENT_MODE_SELECT,
156 .data = { .switchPositions = 3 }
157 }, {
158 .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
159 .mode = ADJUSTMENT_MODE_STEP,
160 .data = { .step = 1 }
161 }, {
162 .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
163 .mode = ADJUSTMENT_MODE_STEP,
164 .data = { .step = 1 }
165 }, {
166 .adjustmentFunction = ADJUSTMENT_PITCH_P,
167 .mode = ADJUSTMENT_MODE_STEP,
168 .data = { .step = 1 }
169 }, {
170 .adjustmentFunction = ADJUSTMENT_PITCH_I,
171 .mode = ADJUSTMENT_MODE_STEP,
172 .data = { .step = 1 }
173 }, {
174 .adjustmentFunction = ADJUSTMENT_PITCH_D,
175 .mode = ADJUSTMENT_MODE_STEP,
176 .data = { .step = 1 }
177 }, {
178 .adjustmentFunction = ADJUSTMENT_ROLL_P,
179 .mode = ADJUSTMENT_MODE_STEP,
180 .data = { .step = 1 }
181 }, {
182 .adjustmentFunction = ADJUSTMENT_ROLL_I,
183 .mode = ADJUSTMENT_MODE_STEP,
184 .data = { .step = 1 }
185 }, {
186 .adjustmentFunction = ADJUSTMENT_ROLL_D,
187 .mode = ADJUSTMENT_MODE_STEP,
188 .data = { .step = 1 }
189 }, {
190 .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
191 .mode = ADJUSTMENT_MODE_STEP,
192 .data = { .step = 1 }
193 }, {
194 .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_F,
195 .mode = ADJUSTMENT_MODE_STEP,
196 .data = { .step = 1 }
197 }, {
198 .adjustmentFunction = ADJUSTMENT_FEEDFORWARD_TRANSITION,
199 .mode = ADJUSTMENT_MODE_STEP,
200 .data = { .step = 1 }
201 }, {
202 .adjustmentFunction = ADJUSTMENT_HORIZON_STRENGTH,
203 .mode = ADJUSTMENT_MODE_SELECT,
204 .data = { .switchPositions = 255 }
205 }, {
206 .adjustmentFunction = ADJUSTMENT_PID_AUDIO,
207 .mode = ADJUSTMENT_MODE_SELECT,
208 .data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) }
209 }, {
210 .adjustmentFunction = ADJUSTMENT_PITCH_F,
211 .mode = ADJUSTMENT_MODE_STEP,
212 .data = { .step = 1 }
213 }, {
214 .adjustmentFunction = ADJUSTMENT_ROLL_F,
215 .mode = ADJUSTMENT_MODE_STEP,
216 .data = { .step = 1 }
217 }, {
218 .adjustmentFunction = ADJUSTMENT_YAW_F,
219 .mode = ADJUSTMENT_MODE_STEP,
220 .data = { .step = 1 }
221 }, {
222 .adjustmentFunction = ADJUSTMENT_OSD_PROFILE,
223 .mode = ADJUSTMENT_MODE_SELECT,
224 .data = { .switchPositions = 3 }
225 }, {
226 .adjustmentFunction = ADJUSTMENT_LED_PROFILE,
227 .mode = ADJUSTMENT_MODE_SELECT,
228 .data = { .switchPositions = 3 }
232 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
233 static const char * const adjustmentLabels[] = {
234 "RC RATE",
235 "RC EXPO",
236 "THROTTLE EXPO",
237 "ROLL RATE",
238 "YAW RATE",
239 "PITCH/ROLL P",
240 "PITCH/ROLL I",
241 "PITCH/ROLL D",
242 "YAW P",
243 "YAW I",
244 "YAW D",
245 "RATE PROFILE",
246 "PITCH RATE",
247 "ROLL RATE",
248 "PITCH P",
249 "PITCH I",
250 "PITCH D",
251 "ROLL P",
252 "ROLL I",
253 "ROLL D",
254 "RC RATE YAW",
255 "PITCH/ROLL F",
256 "FF TRANSITION",
257 "HORIZON STRENGTH",
258 "ROLL RC RATE",
259 "PITCH RC RATE",
260 "ROLL RC EXPO",
261 "PITCH RC EXPO",
262 "PID AUDIO",
263 "PITCH F",
264 "ROLL F",
265 "YAW F",
266 "OSD PROFILE",
269 static int adjustmentRangeNameIndex = 0;
270 static int adjustmentRangeValue = -1;
271 #endif
273 static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
275 beeperConfirmationBeeps(delta > 0 ? 2 : 1);
276 int newValue;
277 switch (adjustmentFunction) {
278 case ADJUSTMENT_RC_RATE:
279 case ADJUSTMENT_ROLL_RC_RATE:
280 newValue = constrain((int)controlRateConfig->rcRates[FD_ROLL] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
281 controlRateConfig->rcRates[FD_ROLL] = newValue;
282 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue);
283 if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) {
284 break;
286 // fall through for combined ADJUSTMENT_RC_EXPO
287 FALLTHROUGH;
288 case ADJUSTMENT_PITCH_RC_RATE:
289 newValue = constrain((int)controlRateConfig->rcRates[FD_PITCH] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
290 controlRateConfig->rcRates[FD_PITCH] = newValue;
291 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue);
292 break;
293 case ADJUSTMENT_RC_EXPO:
294 case ADJUSTMENT_ROLL_RC_EXPO:
295 newValue = constrain((int)controlRateConfig->rcExpo[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
296 controlRateConfig->rcExpo[FD_ROLL] = newValue;
297 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue);
298 if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) {
299 break;
301 // fall through for combined ADJUSTMENT_RC_EXPO
302 FALLTHROUGH;
303 case ADJUSTMENT_PITCH_RC_EXPO:
304 newValue = constrain((int)controlRateConfig->rcExpo[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
305 controlRateConfig->rcExpo[FD_PITCH] = newValue;
306 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue);
307 break;
308 case ADJUSTMENT_THROTTLE_EXPO:
309 newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
310 controlRateConfig->thrExpo8 = newValue;
311 initRcProcessing();
312 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
313 break;
314 case ADJUSTMENT_PITCH_ROLL_RATE:
315 case ADJUSTMENT_PITCH_RATE:
316 newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
317 controlRateConfig->rates[FD_PITCH] = newValue;
318 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
319 if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
320 break;
322 // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
323 FALLTHROUGH;
324 case ADJUSTMENT_ROLL_RATE:
325 newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
326 controlRateConfig->rates[FD_ROLL] = newValue;
327 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
328 break;
329 case ADJUSTMENT_YAW_RATE:
330 newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_RATE_MAX);
331 controlRateConfig->rates[FD_YAW] = newValue;
332 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
333 break;
334 case ADJUSTMENT_PITCH_ROLL_P:
335 case ADJUSTMENT_PITCH_P:
336 newValue = constrain((int)currentPidProfile->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
337 currentPidProfile->pid[PID_PITCH].P = newValue;
338 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
340 if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
341 break;
343 // fall through for combined ADJUSTMENT_PITCH_ROLL_P
344 FALLTHROUGH;
345 case ADJUSTMENT_ROLL_P:
346 newValue = constrain((int)currentPidProfile->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
347 currentPidProfile->pid[PID_ROLL].P = newValue;
348 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
349 break;
350 case ADJUSTMENT_PITCH_ROLL_I:
351 case ADJUSTMENT_PITCH_I:
352 newValue = constrain((int)currentPidProfile->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
353 currentPidProfile->pid[PID_PITCH].I = newValue;
354 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
355 if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
356 break;
358 // fall through for combined ADJUSTMENT_PITCH_ROLL_I
359 FALLTHROUGH;
360 case ADJUSTMENT_ROLL_I:
361 newValue = constrain((int)currentPidProfile->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
362 currentPidProfile->pid[PID_ROLL].I = newValue;
363 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
364 break;
365 case ADJUSTMENT_PITCH_ROLL_D:
366 case ADJUSTMENT_PITCH_D:
367 newValue = constrain((int)currentPidProfile->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
368 currentPidProfile->pid[PID_PITCH].D = newValue;
369 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
370 if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
371 break;
373 // fall through for combined ADJUSTMENT_PITCH_ROLL_D
374 FALLTHROUGH;
375 case ADJUSTMENT_ROLL_D:
376 newValue = constrain((int)currentPidProfile->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
377 currentPidProfile->pid[PID_ROLL].D = newValue;
378 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
379 break;
380 case ADJUSTMENT_YAW_P:
381 newValue = constrain((int)currentPidProfile->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in cli.c
382 currentPidProfile->pid[PID_YAW].P = newValue;
383 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
384 break;
385 case ADJUSTMENT_YAW_I:
386 newValue = constrain((int)currentPidProfile->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in cli.c
387 currentPidProfile->pid[PID_YAW].I = newValue;
388 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
389 break;
390 case ADJUSTMENT_YAW_D:
391 newValue = constrain((int)currentPidProfile->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in cli.c
392 currentPidProfile->pid[PID_YAW].D = newValue;
393 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
394 break;
395 case ADJUSTMENT_RC_RATE_YAW:
396 newValue = constrain((int)controlRateConfig->rcRates[FD_YAW] + delta, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
397 controlRateConfig->rcRates[FD_YAW] = newValue;
398 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
399 break;
400 case ADJUSTMENT_PITCH_ROLL_F:
401 case ADJUSTMENT_PITCH_F:
402 newValue = constrain(currentPidProfile->pid[PID_PITCH].F + delta, 0, 2000);
403 currentPidProfile->pid[PID_PITCH].F = newValue;
404 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
406 if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
407 break;
409 // fall through for combined ADJUSTMENT_PITCH_ROLL_F
410 FALLTHROUGH;
411 case ADJUSTMENT_ROLL_F:
412 newValue = constrain(currentPidProfile->pid[PID_ROLL].F + delta, 0, 2000);
413 currentPidProfile->pid[PID_ROLL].F = newValue;
414 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
415 break;
416 case ADJUSTMENT_YAW_F:
417 newValue = constrain(currentPidProfile->pid[PID_YAW].F + delta, 0, 2000);
418 currentPidProfile->pid[PID_YAW].F = newValue;
419 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
420 break;
421 #if defined(USE_FEEDFORWARD)
422 case ADJUSTMENT_FEEDFORWARD_TRANSITION:
423 newValue = constrain(currentPidProfile->feedforward_transition + delta, 1, 100); // FIXME magic numbers repeated in cli.c
424 currentPidProfile->feedforward_transition = newValue;
425 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
426 break;
427 #endif
428 default:
429 newValue = -1;
430 break;
433 return newValue;
436 static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjustmentFunction_e adjustmentFunction, int value)
438 int newValue;
440 switch (adjustmentFunction) {
441 case ADJUSTMENT_RC_RATE:
442 case ADJUSTMENT_ROLL_RC_RATE:
443 newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
444 controlRateConfig->rcRates[FD_ROLL] = newValue;
445 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_RATE, newValue);
446 if (adjustmentFunction == ADJUSTMENT_ROLL_RC_RATE) {
447 break;
449 // fall through for combined ADJUSTMENT_RC_EXPO
450 FALLTHROUGH;
451 case ADJUSTMENT_PITCH_RC_RATE:
452 newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
453 controlRateConfig->rcRates[FD_PITCH] = newValue;
454 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_RATE, newValue);
455 break;
456 case ADJUSTMENT_RC_EXPO:
457 case ADJUSTMENT_ROLL_RC_EXPO:
458 newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
459 controlRateConfig->rcExpo[FD_ROLL] = newValue;
460 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RC_EXPO, newValue);
461 if (adjustmentFunction == ADJUSTMENT_ROLL_RC_EXPO) {
462 break;
464 // fall through for combined ADJUSTMENT_RC_EXPO
465 FALLTHROUGH;
466 case ADJUSTMENT_PITCH_RC_EXPO:
467 newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX);
468 controlRateConfig->rcExpo[FD_PITCH] = newValue;
469 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RC_EXPO, newValue);
470 break;
471 case ADJUSTMENT_THROTTLE_EXPO:
472 newValue = constrain(value, 0, 100); // FIXME magic numbers repeated in cli.c
473 controlRateConfig->thrExpo8 = newValue;
474 initRcProcessing();
475 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
476 break;
477 case ADJUSTMENT_PITCH_ROLL_RATE:
478 case ADJUSTMENT_PITCH_RATE:
479 newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
480 controlRateConfig->rates[FD_PITCH] = newValue;
481 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
482 if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
483 break;
485 // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
486 FALLTHROUGH;
487 case ADJUSTMENT_ROLL_RATE:
488 newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
489 controlRateConfig->rates[FD_ROLL] = newValue;
490 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
491 break;
492 case ADJUSTMENT_YAW_RATE:
493 newValue = constrain(value, 0, CONTROL_RATE_CONFIG_RATE_MAX);
494 controlRateConfig->rates[FD_YAW] = newValue;
495 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
496 break;
497 case ADJUSTMENT_PITCH_ROLL_P:
498 case ADJUSTMENT_PITCH_P:
499 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
500 currentPidProfile->pid[PID_PITCH].P = newValue;
501 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
503 if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
504 break;
506 // fall through for combined ADJUSTMENT_PITCH_ROLL_P
507 FALLTHROUGH;
508 case ADJUSTMENT_ROLL_P:
509 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
510 currentPidProfile->pid[PID_ROLL].P = newValue;
511 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
512 break;
513 case ADJUSTMENT_PITCH_ROLL_I:
514 case ADJUSTMENT_PITCH_I:
515 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
516 currentPidProfile->pid[PID_PITCH].I = newValue;
517 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
518 if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
519 break;
521 // fall through for combined ADJUSTMENT_PITCH_ROLL_I
522 FALLTHROUGH;
523 case ADJUSTMENT_ROLL_I:
524 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
525 currentPidProfile->pid[PID_ROLL].I = newValue;
526 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
527 break;
528 case ADJUSTMENT_PITCH_ROLL_D:
529 case ADJUSTMENT_PITCH_D:
530 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
531 currentPidProfile->pid[PID_PITCH].D = newValue;
532 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
533 if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
534 break;
536 // fall through for combined ADJUSTMENT_PITCH_ROLL_D
537 FALLTHROUGH;
538 case ADJUSTMENT_ROLL_D:
539 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
540 currentPidProfile->pid[PID_ROLL].D = newValue;
541 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
542 break;
543 case ADJUSTMENT_YAW_P:
544 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
545 currentPidProfile->pid[PID_YAW].P = newValue;
546 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
547 break;
548 case ADJUSTMENT_YAW_I:
549 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
550 currentPidProfile->pid[PID_YAW].I = newValue;
551 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
552 break;
553 case ADJUSTMENT_YAW_D:
554 newValue = constrain(value, 0, 200); // FIXME magic numbers repeated in cli.c
555 currentPidProfile->pid[PID_YAW].D = newValue;
556 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
557 break;
558 case ADJUSTMENT_RC_RATE_YAW:
559 newValue = constrain(value, 1, CONTROL_RATE_CONFIG_RC_RATES_MAX);
560 controlRateConfig->rcRates[FD_YAW] = newValue;
561 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
562 break;
563 case ADJUSTMENT_PITCH_ROLL_F:
564 case ADJUSTMENT_PITCH_F:
565 newValue = constrain(value, 0, 2000);
566 currentPidProfile->pid[PID_PITCH].F = newValue;
567 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_F, newValue);
569 if (adjustmentFunction == ADJUSTMENT_PITCH_F) {
570 break;
572 // fall through for combined ADJUSTMENT_PITCH_ROLL_F
573 FALLTHROUGH;
574 case ADJUSTMENT_ROLL_F:
575 newValue = constrain(value, 0, 2000);
576 currentPidProfile->pid[PID_ROLL].F = newValue;
577 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F, newValue);
578 break;
579 case ADJUSTMENT_YAW_F:
580 newValue = constrain(value, 0, 2000);
581 currentPidProfile->pid[PID_YAW].F = newValue;
582 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F, newValue);
583 break;
584 #if defined(USE_FEEDFORWARD)
585 case ADJUSTMENT_FEEDFORWARD_TRANSITION:
586 newValue = constrain(value, 1, 100); // FIXME magic numbers repeated in cli.c
587 currentPidProfile->feedforward_transition = newValue;
588 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_FEEDFORWARD_TRANSITION, newValue);
589 break;
590 #endif
591 default:
592 newValue = -1;
593 break;
596 return newValue;
599 static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction, uint8_t position)
601 uint8_t beeps = 0;
603 switch (adjustmentFunction) {
604 case ADJUSTMENT_RATE_PROFILE:
605 if (getCurrentControlRateProfileIndex() != position) {
606 changeControlRateProfile(position);
607 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
609 beeps = position + 1;
611 break;
612 case ADJUSTMENT_HORIZON_STRENGTH:
614 uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c
615 if (currentPidProfile->pid[PID_LEVEL].D != newValue) {
616 beeps = ((newValue - currentPidProfile->pid[PID_LEVEL].D) / 8) + 1;
617 currentPidProfile->pid[PID_LEVEL].D = newValue;
618 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position);
621 break;
622 case ADJUSTMENT_PID_AUDIO:
623 #ifdef USE_PID_AUDIO
625 pidAudioModes_e newMode = pidAudioPositionToModeMap[position];
626 if (newMode != pidAudioGetMode()) {
627 pidAudioSetMode(newMode);
630 #endif
631 break;
632 case ADJUSTMENT_OSD_PROFILE:
633 #ifdef USE_OSD_PROFILES
634 if (getCurrentOsdProfileIndex() != (position + 1)) {
635 changeOsdProfileIndex(position+1);
637 #endif
638 break;
639 case ADJUSTMENT_LED_PROFILE:
640 #ifdef USE_LED_STRIP
641 if (getLedProfile() != position) {
642 setLedProfile(position);
644 #endif
645 break;
647 default:
648 break;
651 if (beeps) {
652 beeperConfirmationBeeps(beeps);
655 return position;
658 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
660 static void calcActiveAdjustmentRanges(void)
662 // This initialisation upsets the scheduler task duration estimation
663 schedulerIgnoreTaskExecTime();
665 adjustmentRange_t defaultAdjustmentRange;
666 memset(&defaultAdjustmentRange, 0, sizeof(defaultAdjustmentRange));
668 stepwiseAdjustmentCount = 0;
669 continuosAdjustmentCount = 0;
670 for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
671 const adjustmentRange_t * const adjustmentRange = adjustmentRanges(i);
672 if (memcmp(adjustmentRange, &defaultAdjustmentRange, sizeof(defaultAdjustmentRange)) != 0) {
673 const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
674 if (adjustmentRange->adjustmentCenter == 0 && adjustmentConfig->mode != ADJUSTMENT_MODE_SELECT) {
675 timedAdjustmentState_t *adjustmentState = &stepwiseAdjustments[stepwiseAdjustmentCount++];
676 adjustmentState->adjustmentRangeIndex = i;
677 adjustmentState->timeoutAt = 0;
678 adjustmentState->ready = true;
679 } else {
680 continuosAdjustmentState_t *adjustmentState = &continuosAdjustments[continuosAdjustmentCount++];
681 adjustmentState->adjustmentRangeIndex = i;
682 adjustmentState->lastRcData = 0;
688 #define VALUE_DISPLAY_LATENCY_MS 2000
690 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
691 static void updateOsdAdjustmentData(int newValue, adjustmentFunction_e adjustmentFunction)
693 static timeMs_t lastValueChangeMs;
695 timeMs_t currentTimeMs = millis();
696 if (newValue != -1
697 && adjustmentFunction != ADJUSTMENT_RATE_PROFILE // Rate profile already has an OSD element
698 #ifdef USE_OSD_PROFILES
699 && adjustmentFunction != ADJUSTMENT_OSD_PROFILE
700 #endif
702 adjustmentRangeNameIndex = adjustmentFunction;
703 adjustmentRangeValue = newValue;
705 lastValueChangeMs = currentTimeMs;
708 if (cmp32(currentTimeMs, lastValueChangeMs + VALUE_DISPLAY_LATENCY_MS) >= 0) {
709 adjustmentRangeNameIndex = 0;
712 #endif
714 #define RESET_FREQUENCY_2HZ (1000 / 2)
716 static void processStepwiseAdjustments(controlRateConfig_t *controlRateConfig, const bool canUseRxData)
718 const timeMs_t now = millis();
720 for (int index = 0; index < stepwiseAdjustmentCount; index++) {
721 timedAdjustmentState_t *adjustmentState = &stepwiseAdjustments[index];
722 const adjustmentRange_t *const adjustmentRange = adjustmentRanges(adjustmentState->adjustmentRangeIndex);
723 const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
724 const adjustmentFunction_e adjustmentFunction = adjustmentConfig->adjustmentFunction;
726 if (!isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) ||
727 adjustmentFunction == ADJUSTMENT_NONE) {
728 adjustmentState->timeoutAt = 0;
730 continue;
733 if (cmp32(now, adjustmentState->timeoutAt) >= 0) {
734 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
735 adjustmentState->ready = true;
738 if (!canUseRxData) {
739 continue;
742 const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex;
744 if (adjustmentConfig->mode == ADJUSTMENT_MODE_STEP) {
745 int delta;
746 if (rcData[channelIndex] > rxConfig()->midrc + 200) {
747 delta = adjustmentConfig->data.step;
748 } else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
749 delta = -adjustmentConfig->data.step;
750 } else {
751 // returning the switch to the middle immediately resets the ready state
752 adjustmentState->ready = true;
753 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
754 continue;
756 if (!adjustmentState->ready) {
757 continue;
760 int newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
762 setConfigDirty();
764 pidInitConfig(currentPidProfile);
766 adjustmentState->ready = false;
768 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
769 updateOsdAdjustmentData(newValue, adjustmentConfig->adjustmentFunction);
770 #else
771 UNUSED(newValue);
772 #endif
777 static void setConfigDirtyIfNotPermanent(const channelRange_t *range)
779 if (!(range->startStep == MIN_MODE_RANGE_STEP && range->endStep == MAX_MODE_RANGE_STEP)) {
780 // Only set the configuration dirty if this range is NOT permanently enabled (and the config thus never used).
781 setConfigDirty();
785 static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig)
787 for (int i = 0; i < continuosAdjustmentCount; i++) {
788 continuosAdjustmentState_t *adjustmentState = &continuosAdjustments[i];
789 const adjustmentRange_t * const adjustmentRange = adjustmentRanges(adjustmentState->adjustmentRangeIndex);
790 const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentRange->auxSwitchChannelIndex;
791 const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentConfig - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
792 const adjustmentFunction_e adjustmentFunction = adjustmentConfig->adjustmentFunction;
794 if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range) &&
795 adjustmentFunction != ADJUSTMENT_NONE) {
797 if (rcData[channelIndex] != adjustmentState->lastRcData) {
798 int newValue = -1;
800 if (adjustmentConfig->mode == ADJUSTMENT_MODE_SELECT) {
801 int switchPositions = adjustmentConfig->data.switchPositions;
802 if (adjustmentFunction == ADJUSTMENT_RATE_PROFILE && systemConfig()->rateProfile6PosSwitch) {
803 switchPositions = 6;
805 const uint16_t rangeWidth = (2100 - 900) / switchPositions;
806 const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
807 newValue = applySelectAdjustment(adjustmentFunction, position);
809 setConfigDirtyIfNotPermanent(&adjustmentRange->range);
810 } else {
811 // If setting is defined for step adjustment and center value has been specified, apply values directly (scaled) from aux channel
812 if (adjustmentRange->adjustmentCenter &&
813 (adjustmentConfig->mode == ADJUSTMENT_MODE_STEP)) {
814 int value = (((rcData[channelIndex] - PWM_RANGE_MIDDLE) * adjustmentRange->adjustmentScale) / (PWM_RANGE_MIDDLE - PWM_RANGE_MIN)) + adjustmentRange->adjustmentCenter;
816 newValue = applyAbsoluteAdjustment(controlRateConfig, adjustmentFunction, value);
818 setConfigDirtyIfNotPermanent(&adjustmentRange->range);
820 pidInitConfig(currentPidProfile);
823 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
824 updateOsdAdjustmentData(newValue, adjustmentConfig->adjustmentFunction);
825 #else
826 UNUSED(newValue);
827 #endif
828 adjustmentState->lastRcData = rcData[channelIndex];
830 } else {
831 adjustmentState->lastRcData = 0;
836 void processRcAdjustments(controlRateConfig_t *controlRateConfig)
838 const bool canUseRxData = rxIsReceivingSignal();
840 // Recalculate the new active adjustments if required
841 if (stepwiseAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) {
842 // This can take up to 30us and is only call when not armed so ignore this timing as it doesn't impact flight
843 schedulerIgnoreTaskExecTime();
844 calcActiveAdjustmentRanges();
847 processStepwiseAdjustments(controlRateConfig, canUseRxData);
849 if (canUseRxData) {
850 processContinuosAdjustments(controlRateConfig);
853 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
854 // Hide the element if there is no change
855 updateOsdAdjustmentData(-1, 0);
856 #endif
859 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
860 const char *getAdjustmentsRangeName(void)
862 if (adjustmentRangeNameIndex > 0) {
863 return &adjustmentLabels[adjustmentRangeNameIndex - 1][0];
864 } else {
865 return NULL;
869 int getAdjustmentsRangeValue(void)
871 return adjustmentRangeValue;
873 #endif
875 void activeAdjustmentRangeReset(void)
877 stepwiseAdjustmentCount = ADJUSTMENT_RANGE_COUNT_INVALID;