src/main/fc/rc_adjustments.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include <math.h>
  23
  24 #include "platform.h"
  25
  26 #include "blackbox/blackbox.h"
  27
  28 #include "build/build_config.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/system.h"
  35
  36 #include "config/parameter_group.h"
  37 #include "config/parameter_group_ids.h"
  38 #include "config/feature.h"
  39
  40 #include "flight/pid.h"
  41
  42 #include "io/beeper.h"
  43 #include "io/motors.h"
  44
  45 #include "fc/config.h"
  46 #include "fc/controlrate_profile.h"
  47 #include "fc/rc_adjustments.h"
  48 #include "fc/rc_controls.h"
  49 #include "fc/fc_rc.h"
  50
  51 #include "rx/rx.h"
  52
  53 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
  54
  55 static pidProfile_t *pidProfile;
  56
  57 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
  58 {
  59 #ifndef BLACKBOX
  60     UNUSED(adjustmentFunction);
  61     UNUSED(newValue);
  62 #else
  63     if (blackboxConfig()->device) {
  64         flightLogEvent_inflightAdjustment_t eventData;
  65         eventData.adjustmentFunction = adjustmentFunction;
  66         eventData.newValue = newValue;
  67         eventData.floatFlag = false;
  68         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
  69     }
  70 #endif
  71 }
  72
  73 #if 0
  74 static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue)
  75 {
  76 #ifndef BLACKBOX
  77     UNUSED(adjustmentFunction);
  78     UNUSED(newFloatValue);
  79 #else
  80     if (blackboxConfig()->device) {
  81         flightLogEvent_inflightAdjustment_t eventData;
  82         eventData.adjustmentFunction = adjustmentFunction;
  83         eventData.newFloatValue = newFloatValue;
  84         eventData.floatFlag = true;
  85         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
  86     }
  87 #endif
  88 }
  89 #endif
  90
  91 static uint8_t adjustmentStateMask = 0;
  92
  93 #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex)
  94 #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex)
  95
  96 #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex))
  97
  98 // sync with adjustmentFunction_e
  99 static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = {
 100     {
 101         .adjustmentFunction = ADJUSTMENT_RC_RATE,
 102         .mode = ADJUSTMENT_MODE_STEP,
 103         .data = { .stepConfig = { .step = 1 }}
 104     },
 105     {
 106         .adjustmentFunction = ADJUSTMENT_RC_EXPO,
 107         .mode = ADJUSTMENT_MODE_STEP,
 108         .data = { .stepConfig = { .step = 1 }}
 109     },
 110     {
 111         .adjustmentFunction = ADJUSTMENT_THROTTLE_EXPO,
 112         .mode = ADJUSTMENT_MODE_STEP,
 113         .data = { .stepConfig = { .step = 1 }}
 114     },
 115     {
 116         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_RATE,
 117         .mode = ADJUSTMENT_MODE_STEP,
 118         .data = { .stepConfig = { .step = 1 }}
 119     },
 120     {
 121         .adjustmentFunction = ADJUSTMENT_YAW_RATE,
 122         .mode = ADJUSTMENT_MODE_STEP,
 123         .data = { .stepConfig = { .step = 1 }}
 124     },
 125     {
 126         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
 127         .mode = ADJUSTMENT_MODE_STEP,
 128         .data = { .stepConfig = { .step = 1 }}
 129     },
 130     {
 131         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
 132         .mode = ADJUSTMENT_MODE_STEP,
 133         .data = { .stepConfig = { .step = 1 }}
 134     },
 135     {
 136         .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
 137         .mode = ADJUSTMENT_MODE_STEP,
 138         .data = { .stepConfig = { .step = 1 }}
 139     },
 140     {
 141         .adjustmentFunction = ADJUSTMENT_YAW_P,
 142         .mode = ADJUSTMENT_MODE_STEP,
 143         .data = { .stepConfig = { .step = 1 }}
 144     },
 145     {
 146         .adjustmentFunction = ADJUSTMENT_YAW_I,
 147         .mode = ADJUSTMENT_MODE_STEP,
 148         .data = { .stepConfig = { .step = 1 }}
 149     },
 150     {
 151         .adjustmentFunction = ADJUSTMENT_YAW_D,
 152         .mode = ADJUSTMENT_MODE_STEP,
 153         .data = { .stepConfig = { .step = 1 }}
 154     },
 155     {
 156         .adjustmentFunction = ADJUSTMENT_RATE_PROFILE,
 157         .mode = ADJUSTMENT_MODE_SELECT,
 158         .data = { .selectConfig = { .switchPositions = 3 }}
 159     },
 160     {
 161         .adjustmentFunction = ADJUSTMENT_PITCH_RATE,
 162         .mode = ADJUSTMENT_MODE_STEP,
 163         .data = { .stepConfig = { .step = 1 }}
 164     },
 165     {
 166         .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
 167         .mode = ADJUSTMENT_MODE_STEP,
 168         .data = { .stepConfig = { .step = 1 }}
 169     },
 170     {
 171         .adjustmentFunction = ADJUSTMENT_PITCH_P,
 172         .mode = ADJUSTMENT_MODE_STEP,
 173         .data = { .stepConfig = { .step = 1 }}
 174     },
 175     {
 176         .adjustmentFunction = ADJUSTMENT_PITCH_I,
 177         .mode = ADJUSTMENT_MODE_STEP,
 178         .data = { .stepConfig = { .step = 1 }}
 179     },
 180     {
 181         .adjustmentFunction = ADJUSTMENT_PITCH_D,
 182         .mode = ADJUSTMENT_MODE_STEP,
 183         .data = { .stepConfig = { .step = 1 }}
 184     },
 185     {
 186         .adjustmentFunction = ADJUSTMENT_ROLL_P,
 187         .mode = ADJUSTMENT_MODE_STEP,
 188         .data = { .stepConfig = { .step = 1 }}
 189     },
 190     {
 191         .adjustmentFunction = ADJUSTMENT_ROLL_I,
 192         .mode = ADJUSTMENT_MODE_STEP,
 193         .data = { .stepConfig = { .step = 1 }}
 194     },
 195     {
 196         .adjustmentFunction = ADJUSTMENT_ROLL_D,
 197         .mode = ADJUSTMENT_MODE_STEP,
 198         .data = { .stepConfig = { .step = 1 }}
 199     },
 200     {
 201         .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
 202         .mode = ADJUSTMENT_MODE_STEP,
 203         .data = { .stepConfig = { .step = 1 }}
 204     },
 205     {
 206         .adjustmentFunction = ADJUSTMENT_D_SETPOINT,
 207         .mode = ADJUSTMENT_MODE_STEP,
 208         .data = { .stepConfig = { .step = 1 }}
 209     },
 210     {
 211         .adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
 212         .mode = ADJUSTMENT_MODE_STEP,
 213         .data = { .stepConfig = { .step = 1 }}
 214     },
 215     {
 216         .adjustmentFunction = ADJUSTMENT_HORIZON_STRENGTH,
 217         .mode = ADJUSTMENT_MODE_SELECT,
 218         .data = { .selectConfig = { .switchPositions = 255 }}
 219
 220     }
 221 };
 222
 223 #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1
 224
 225 static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
 226
 227 static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig)
 228 {
 229     adjustmentState_t *adjustmentState = &adjustmentStates[index];
 230
 231     if (adjustmentState->config == adjustmentConfig) {
 232         // already configured
 233         return;
 234     }
 235     adjustmentState->auxChannelIndex = auxSwitchChannelIndex;
 236     adjustmentState->config = adjustmentConfig;
 237     adjustmentState->timeoutAt = 0;
 238
 239     MARK_ADJUSTMENT_FUNCTION_AS_READY(index);
 240 }
 241
 242 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
 243 {
 244     int newValue;
 245
 246     if (delta > 0) {
 247         beeperConfirmationBeeps(2);
 248     } else {
 249         beeperConfirmationBeeps(1);
 250     }
 251     switch(adjustmentFunction) {
 252         case ADJUSTMENT_RC_RATE:
 253             newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in cli.c
 254             controlRateConfig->rcRate8 = newValue;
 255             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
 256             break;
 257         case ADJUSTMENT_RC_EXPO:
 258             newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
 259             controlRateConfig->rcExpo8 = newValue;
 260             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
 261             break;
 262         case ADJUSTMENT_THROTTLE_EXPO:
 263             newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c
 264             controlRateConfig->thrExpo8 = newValue;
 265             generateThrottleCurve();
 266             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
 267             break;
 268         case ADJUSTMENT_PITCH_ROLL_RATE:
 269         case ADJUSTMENT_PITCH_RATE:
 270             newValue = constrain((int)controlRateConfig->rates[FD_PITCH] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
 271             controlRateConfig->rates[FD_PITCH] = newValue;
 272             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue);
 273             if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) {
 274                 break;
 275             }
 276             // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE
 277         case ADJUSTMENT_ROLL_RATE:
 278             newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
 279             controlRateConfig->rates[FD_ROLL] = newValue;
 280             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue);
 281             break;
 282         case ADJUSTMENT_YAW_RATE:
 283             newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX);
 284             controlRateConfig->rates[FD_YAW] = newValue;
 285             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue);
 286             break;
 287         case ADJUSTMENT_PITCH_ROLL_P:
 288         case ADJUSTMENT_PITCH_P:
 289             newValue = constrain((int)pidProfile->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 290             pidProfile->P8[PIDPITCH] = newValue;
 291             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
 292
 293             if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
 294                 break;
 295             }
 296             // follow though for combined ADJUSTMENT_PITCH_ROLL_P
 297         case ADJUSTMENT_ROLL_P:
 298             newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 299             pidProfile->P8[PIDROLL] = newValue;
 300             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
 301             break;
 302         case ADJUSTMENT_PITCH_ROLL_I:
 303         case ADJUSTMENT_PITCH_I:
 304             newValue = constrain((int)pidProfile->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 305             pidProfile->I8[PIDPITCH] = newValue;
 306             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
 307
 308             if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
 309                 break;
 310             }
 311             // follow though for combined ADJUSTMENT_PITCH_ROLL_I
 312         case ADJUSTMENT_ROLL_I:
 313             newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 314             pidProfile->I8[PIDROLL] = newValue;
 315             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
 316             break;
 317         case ADJUSTMENT_PITCH_ROLL_D:
 318         case ADJUSTMENT_PITCH_D:
 319             newValue = constrain((int)pidProfile->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 320             pidProfile->D8[PIDPITCH] = newValue;
 321             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
 322
 323             if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
 324                 break;
 325             }
 326             // follow though for combined ADJUSTMENT_PITCH_ROLL_D
 327         case ADJUSTMENT_ROLL_D:
 328             newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 329             pidProfile->D8[PIDROLL] = newValue;
 330             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
 331             break;
 332         case ADJUSTMENT_YAW_P:
 333             newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 334             pidProfile->P8[PIDYAW] = newValue;
 335             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
 336             break;
 337         case ADJUSTMENT_YAW_I:
 338             newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 339             pidProfile->I8[PIDYAW] = newValue;
 340             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
 341             break;
 342         case ADJUSTMENT_YAW_D:
 343             newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in cli.c
 344             pidProfile->D8[PIDYAW] = newValue;
 345             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
 346             break;
 347         case ADJUSTMENT_RC_RATE_YAW:
 348             newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in cli.c
 349             controlRateConfig->rcYawRate8 = newValue;
 350             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
 351             break;
 352         case ADJUSTMENT_D_SETPOINT:
 353             newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c
 354             pidProfile->dtermSetpointWeight = newValue;
 355             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
 356             break;
 357         case ADJUSTMENT_D_SETPOINT_TRANSITION:
 358             newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in cli.c
 359             pidProfile->setpointRelaxRatio = newValue;
 360             blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
 361             break;
 362         default:
 363             break;
 364     };
 365 }
 366
 367 static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
 368 {
 369     uint8_t beeps = 0;
 370
 371     switch(adjustmentFunction) {
 372         case ADJUSTMENT_RATE_PROFILE:
 373             {
 374                 if (getCurrentControlRateProfileIndex() != position) {
 375                     changeControlRateProfile(position);
 376                     blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position);
 377                     beeps = position + 1;
 378                 }
 379                 break;
 380             }
 381         case ADJUSTMENT_HORIZON_STRENGTH:
 382             {
 383                 uint8_t newValue = constrain(position, 0, 200); // FIXME magic numbers repeated in serial_cli.c
 384                 if(pidProfile->D8[PIDLEVEL] != newValue) {
 385                     beeps = ((newValue - pidProfile->D8[PIDLEVEL]) / 8) + 1;
 386                     pidProfile->D8[PIDLEVEL] = newValue;
 387                     blackboxLogInflightAdjustmentEvent(ADJUSTMENT_HORIZON_STRENGTH, position);
 388                 }
 389                 break;
 390             }
 391     }
 392
 393     if (beeps) {
 394         beeperConfirmationBeeps(beeps);
 395     }
 396
 397 }
 398
 399 #define RESET_FREQUENCY_2HZ (1000 / 2)
 400
 401 void processRcAdjustments(controlRateConfig_t *controlRateConfig)
 402 {
 403     const uint32_t now = millis();
 404
 405     const bool canUseRxData = rxIsReceivingSignal();
 406
 407     for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
 408         adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
 409
 410         if (!adjustmentState->config) {
 411             continue;
 412         }
 413         const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
 414         if (adjustmentFunction == ADJUSTMENT_NONE) {
 415             continue;
 416         }
 417
 418         const int32_t signedDiff = now - adjustmentState->timeoutAt;
 419         const bool canResetReadyStates = signedDiff >= 0L;
 420
 421         if (canResetReadyStates) {
 422             adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 423             MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 424         }
 425
 426         if (!canUseRxData) {
 427             continue;
 428         }
 429
 430         const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
 431
 432         if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
 433             int delta;
 434             if (rcData[channelIndex] > rxConfig()->midrc + 200) {
 435                 delta = adjustmentState->config->data.stepConfig.step;
 436             } else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
 437                 delta = 0 - adjustmentState->config->data.stepConfig.step;
 438             } else {
 439                 // returning the switch to the middle immediately resets the ready state
 440                 MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
 441                 adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
 442                 continue;
 443             }
 444             if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
 445                 continue;
 446             }
 447
 448             applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
 449             pidInitConfig(pidProfile);
 450         } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
 451             const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
 452             const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
 453             applySelectAdjustment(adjustmentFunction, position);
 454         }
 455         MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
 456     }
 457 }
 458
 459 void resetAdjustmentStates(void)
 460 {
 461     memset(adjustmentStates, 0, sizeof(adjustmentStates));
 462 }
 463
 464 void updateAdjustmentStates(void)
 465 {
 466     for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) {
 467         const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index);
 468         if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) {
 469             const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET];
 470             configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig);
 471         }
 472     }
 473 }
 474
 475 void useAdjustmentConfig(pidProfile_t *pidProfileToUse)
 476 {
 477     pidProfile = pidProfileToUse;
 478 }