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)
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/>.
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"
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"
57 #include "pg/pg_ids.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
)
94 UNUSED(adjustmentFunction
);
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
);
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 }
114 .adjustmentFunction
= ADJUSTMENT_RC_EXPO
,
115 .mode
= ADJUSTMENT_MODE_STEP
,
116 .data
= { .step
= 1 }
118 .adjustmentFunction
= ADJUSTMENT_THROTTLE_EXPO
,
119 .mode
= ADJUSTMENT_MODE_STEP
,
120 .data
= { .step
= 1 }
122 .adjustmentFunction
= ADJUSTMENT_PITCH_ROLL_RATE
,
123 .mode
= ADJUSTMENT_MODE_STEP
,
124 .data
= { .step
= 1 }
126 .adjustmentFunction
= ADJUSTMENT_YAW_RATE
,
127 .mode
= ADJUSTMENT_MODE_STEP
,
128 .data
= { .step
= 1 }
130 .adjustmentFunction
= ADJUSTMENT_PITCH_ROLL_P
,
131 .mode
= ADJUSTMENT_MODE_STEP
,
132 .data
= { .step
= 1 }
134 .adjustmentFunction
= ADJUSTMENT_PITCH_ROLL_I
,
135 .mode
= ADJUSTMENT_MODE_STEP
,
136 .data
= { .step
= 1 }
138 .adjustmentFunction
= ADJUSTMENT_PITCH_ROLL_D
,
139 .mode
= ADJUSTMENT_MODE_STEP
,
140 .data
= { .step
= 1 }
142 .adjustmentFunction
= ADJUSTMENT_YAW_P
,
143 .mode
= ADJUSTMENT_MODE_STEP
,
144 .data
= { .step
= 1 }
146 .adjustmentFunction
= ADJUSTMENT_YAW_I
,
147 .mode
= ADJUSTMENT_MODE_STEP
,
148 .data
= { .step
= 1 }
150 .adjustmentFunction
= ADJUSTMENT_YAW_D
,
151 .mode
= ADJUSTMENT_MODE_STEP
,
152 .data
= { .step
= 1 }
154 .adjustmentFunction
= ADJUSTMENT_RATE_PROFILE
,
155 .mode
= ADJUSTMENT_MODE_SELECT
,
156 .data
= { .switchPositions
= 3 }
158 .adjustmentFunction
= ADJUSTMENT_PITCH_RATE
,
159 .mode
= ADJUSTMENT_MODE_STEP
,
160 .data
= { .step
= 1 }
162 .adjustmentFunction
= ADJUSTMENT_ROLL_RATE
,
163 .mode
= ADJUSTMENT_MODE_STEP
,
164 .data
= { .step
= 1 }
166 .adjustmentFunction
= ADJUSTMENT_PITCH_P
,
167 .mode
= ADJUSTMENT_MODE_STEP
,
168 .data
= { .step
= 1 }
170 .adjustmentFunction
= ADJUSTMENT_PITCH_I
,
171 .mode
= ADJUSTMENT_MODE_STEP
,
172 .data
= { .step
= 1 }
174 .adjustmentFunction
= ADJUSTMENT_PITCH_D
,
175 .mode
= ADJUSTMENT_MODE_STEP
,
176 .data
= { .step
= 1 }
178 .adjustmentFunction
= ADJUSTMENT_ROLL_P
,
179 .mode
= ADJUSTMENT_MODE_STEP
,
180 .data
= { .step
= 1 }
182 .adjustmentFunction
= ADJUSTMENT_ROLL_I
,
183 .mode
= ADJUSTMENT_MODE_STEP
,
184 .data
= { .step
= 1 }
186 .adjustmentFunction
= ADJUSTMENT_ROLL_D
,
187 .mode
= ADJUSTMENT_MODE_STEP
,
188 .data
= { .step
= 1 }
190 .adjustmentFunction
= ADJUSTMENT_RC_RATE_YAW
,
191 .mode
= ADJUSTMENT_MODE_STEP
,
192 .data
= { .step
= 1 }
194 .adjustmentFunction
= ADJUSTMENT_PITCH_ROLL_F
,
195 .mode
= ADJUSTMENT_MODE_STEP
,
196 .data
= { .step
= 1 }
198 .adjustmentFunction
= ADJUSTMENT_FEEDFORWARD_TRANSITION
,
199 .mode
= ADJUSTMENT_MODE_STEP
,
200 .data
= { .step
= 1 }
202 .adjustmentFunction
= ADJUSTMENT_HORIZON_STRENGTH
,
203 .mode
= ADJUSTMENT_MODE_SELECT
,
204 .data
= { .switchPositions
= 255 }
206 .adjustmentFunction
= ADJUSTMENT_PID_AUDIO
,
207 .mode
= ADJUSTMENT_MODE_SELECT
,
208 .data
= { .switchPositions
= ARRAYLEN(pidAudioPositionToModeMap
) }
210 .adjustmentFunction
= ADJUSTMENT_PITCH_F
,
211 .mode
= ADJUSTMENT_MODE_STEP
,
212 .data
= { .step
= 1 }
214 .adjustmentFunction
= ADJUSTMENT_ROLL_F
,
215 .mode
= ADJUSTMENT_MODE_STEP
,
216 .data
= { .step
= 1 }
218 .adjustmentFunction
= ADJUSTMENT_YAW_F
,
219 .mode
= ADJUSTMENT_MODE_STEP
,
220 .data
= { .step
= 1 }
222 .adjustmentFunction
= ADJUSTMENT_OSD_PROFILE
,
223 .mode
= ADJUSTMENT_MODE_SELECT
,
224 .data
= { .switchPositions
= 3 }
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
[] = {
269 static int adjustmentRangeNameIndex
= 0;
270 static int adjustmentRangeValue
= -1;
273 static int applyStepAdjustment(controlRateConfig_t
*controlRateConfig
, uint8_t adjustmentFunction
, int delta
)
275 beeperConfirmationBeeps(delta
> 0 ? 2 : 1);
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
) {
286 // fall through for combined ADJUSTMENT_RC_EXPO
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
);
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
) {
301 // fall through for combined ADJUSTMENT_RC_EXPO
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
);
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
;
312 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO
, newValue
);
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
) {
322 // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
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
);
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
);
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
) {
343 // fall through for combined ADJUSTMENT_PITCH_ROLL_P
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
);
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
) {
358 // fall through for combined ADJUSTMENT_PITCH_ROLL_I
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
);
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
) {
373 // fall through for combined ADJUSTMENT_PITCH_ROLL_D
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
);
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
);
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
);
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
);
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
);
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
) {
409 // fall through for combined ADJUSTMENT_PITCH_ROLL_F
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
);
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
);
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
);
436 static int applyAbsoluteAdjustment(controlRateConfig_t
*controlRateConfig
, adjustmentFunction_e adjustmentFunction
, int value
)
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
) {
449 // fall through for combined ADJUSTMENT_RC_EXPO
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
);
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
) {
464 // fall through for combined ADJUSTMENT_RC_EXPO
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
);
471 case ADJUSTMENT_THROTTLE_EXPO
:
472 newValue
= constrain(value
, 0, 100); // FIXME magic numbers repeated in cli.c
473 controlRateConfig
->thrExpo8
= newValue
;
475 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO
, newValue
);
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
) {
485 // fall through for combined ADJUSTMENT_PITCH_ROLL_RATE
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
);
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
);
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
) {
506 // fall through for combined ADJUSTMENT_PITCH_ROLL_P
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
);
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
) {
521 // fall through for combined ADJUSTMENT_PITCH_ROLL_I
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
);
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
) {
536 // fall through for combined ADJUSTMENT_PITCH_ROLL_D
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
);
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
);
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
);
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
);
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
);
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
) {
572 // fall through for combined ADJUSTMENT_PITCH_ROLL_F
574 case ADJUSTMENT_ROLL_F
:
575 newValue
= constrain(value
, 0, 2000);
576 currentPidProfile
->pid
[PID_ROLL
].F
= newValue
;
577 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_F
, newValue
);
579 case ADJUSTMENT_YAW_F
:
580 newValue
= constrain(value
, 0, 2000);
581 currentPidProfile
->pid
[PID_YAW
].F
= newValue
;
582 blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_F
, newValue
);
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
);
599 static uint8_t applySelectAdjustment(adjustmentFunction_e adjustmentFunction
, uint8_t position
)
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;
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
);
622 case ADJUSTMENT_PID_AUDIO
:
625 pidAudioModes_e newMode
= pidAudioPositionToModeMap
[position
];
626 if (newMode
!= pidAudioGetMode()) {
627 pidAudioSetMode(newMode
);
632 case ADJUSTMENT_OSD_PROFILE
:
633 #ifdef USE_OSD_PROFILES
634 if (getCurrentOsdProfileIndex() != (position
+ 1)) {
635 changeOsdProfileIndex(position
+1);
639 case ADJUSTMENT_LED_PROFILE
:
641 if (getLedProfile() != position
) {
642 setLedProfile(position
);
652 beeperConfirmationBeeps(beeps
);
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;
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();
697 && adjustmentFunction
!= ADJUSTMENT_RATE_PROFILE
// Rate profile already has an OSD element
698 #ifdef USE_OSD_PROFILES
699 && adjustmentFunction
!= ADJUSTMENT_OSD_PROFILE
702 adjustmentRangeNameIndex
= adjustmentFunction
;
703 adjustmentRangeValue
= newValue
;
705 lastValueChangeMs
= currentTimeMs
;
708 if (cmp32(currentTimeMs
, lastValueChangeMs
+ VALUE_DISPLAY_LATENCY_MS
) >= 0) {
709 adjustmentRangeNameIndex
= 0;
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;
733 if (cmp32(now
, adjustmentState
->timeoutAt
) >= 0) {
734 adjustmentState
->timeoutAt
= now
+ RESET_FREQUENCY_2HZ
;
735 adjustmentState
->ready
= true;
742 const uint8_t channelIndex
= NON_AUX_CHANNEL_COUNT
+ adjustmentRange
->auxSwitchChannelIndex
;
744 if (adjustmentConfig
->mode
== ADJUSTMENT_MODE_STEP
) {
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
;
751 // returning the switch to the middle immediately resets the ready state
752 adjustmentState
->ready
= true;
753 adjustmentState
->timeoutAt
= now
+ RESET_FREQUENCY_2HZ
;
756 if (!adjustmentState
->ready
) {
760 int newValue
= applyStepAdjustment(controlRateConfig
, adjustmentFunction
, delta
);
764 pidInitConfig(currentPidProfile
);
766 adjustmentState
->ready
= false;
768 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
769 updateOsdAdjustmentData(newValue
, adjustmentConfig
->adjustmentFunction
);
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).
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
) {
800 if (adjustmentConfig
->mode
== ADJUSTMENT_MODE_SELECT
) {
801 int switchPositions
= adjustmentConfig
->data
.switchPositions
;
802 if (adjustmentFunction
== ADJUSTMENT_RATE_PROFILE
&& systemConfig()->rateProfile6PosSwitch
) {
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
);
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
);
828 adjustmentState
->lastRcData
= rcData
[channelIndex
];
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
);
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);
859 #if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
860 const char *getAdjustmentsRangeName(void)
862 if (adjustmentRangeNameIndex
> 0) {
863 return &adjustmentLabels
[adjustmentRangeNameIndex
- 1][0];
869 int getAdjustmentsRangeValue(void)
871 return adjustmentRangeValue
;
875 void activeAdjustmentRangeReset(void)
877 stepwiseAdjustmentCount
= ADJUSTMENT_RANGE_COUNT_INVALID
;