src/main/programming/logic_condition.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26
  27 #include "config/config_reset.h"
  28 #include "config/parameter_group.h"
  29 #include "config/parameter_group_ids.h"
  30
  31 #include "programming/logic_condition.h"
  32 #include "programming/global_variables.h"
  33 #include "programming/pid.h"
  34 #include "common/utils.h"
  35 #include "rx/rx.h"
  36 #include "common/maths.h"
  37 #include "fc/config.h"
  38 #include "fc/cli.h"
  39 #include "fc/fc_core.h"
  40 #include "fc/rc_controls.h"
  41 #include "fc/runtime_config.h"
  42 #include "fc/rc_modes.h"
  43 #include "navigation/navigation.h"
  44 #include "sensors/battery.h"
  45 #include "sensors/pitotmeter.h"
  46 #include "sensors/rangefinder.h"
  47 #include "flight/imu.h"
  48 #include "flight/pid.h"
  49 #include "flight/mixer_profile.h"
  50 #include "drivers/io_port_expander.h"
  51 #include "io/osd_common.h"
  52 #include "sensors/diagnostics.h"
  53
  54 #include "navigation/navigation.h"
  55 #include "navigation/navigation_private.h"
  56
  57 #include "io/vtx.h"
  58 #include "drivers/vtx_common.h"
  59 #include "drivers/light_ws2811strip.h"
  60
  61 PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4);
  62
  63 EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags;
  64 EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST];
  65 EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  66 EXTENDED_FASTRAM flightAxisOverride_t flightAxisOverride[XYZ_AXIS_COUNT];
  67
  68 void pgResetFn_logicConditions(logicCondition_t *instance)
  69 {
  70     for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
  71         RESET_CONFIG(logicCondition_t, &instance[i],
  72             .enabled = 0,
  73             .activatorId = -1,
  74             .operation = 0,
  75             .operandA = {
  76                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  77                 .value = 0
  78             },
  79             .operandB = {
  80                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  81                 .value = 0
  82             },
  83             .flags = 0
  84         );
  85     }
  86 }
  87
  88 logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
  89
  90 static int logicConditionCompute(
  91     int32_t currentValue,
  92     logicOperation_e operation,
  93     int32_t operandA,
  94     int32_t operandB,
  95     uint8_t lcIndex
  96 ) {
  97     int temporaryValue;
  98 #if defined(USE_VTX_CONTROL)
  99     vtxDeviceCapability_t vtxDeviceCapability;
 100 #endif
 101
 102     switch (operation) {
 103
 104         case LOGIC_CONDITION_TRUE:
 105             return true;
 106             break;
 107
 108         case LOGIC_CONDITION_EQUAL:
 109             return operandA == operandB;
 110             break;
 111
 112         case LOGIC_CONDITION_APPROX_EQUAL:
 113             {
 114                 uint16_t offest = operandA / 100;
 115                 return ((operandB >= (operandA - offest)) && (operandB <= (operandA + offest)));
 116             }
 117             break;
 118
 119         case LOGIC_CONDITION_GREATER_THAN:
 120             return operandA > operandB;
 121             break;
 122
 123         case LOGIC_CONDITION_LOWER_THAN:
 124             return operandA < operandB;
 125             break;
 126
 127         case LOGIC_CONDITION_LOW:
 128             return operandA < 1333;
 129             break;
 130
 131         case LOGIC_CONDITION_MID:
 132             return operandA >= 1333 && operandA <= 1666;
 133             break;
 134
 135         case LOGIC_CONDITION_HIGH:
 136             return operandA > 1666;
 137             break;
 138
 139         case LOGIC_CONDITION_AND:
 140             return (operandA && operandB);
 141             break;
 142
 143         case LOGIC_CONDITION_OR:
 144             return (operandA || operandB);
 145             break;
 146
 147         case LOGIC_CONDITION_XOR:
 148             return (operandA != operandB);
 149             break;
 150
 151         case LOGIC_CONDITION_NAND:
 152             return !(operandA && operandB);
 153             break;
 154
 155         case LOGIC_CONDITION_NOR:
 156             return !(operandA || operandB);
 157             break;
 158
 159         case LOGIC_CONDITION_NOT:
 160             return !operandA;
 161             break;
 162
 163         case LOGIC_CONDITION_STICKY:
 164             // Operand A is activation operator
 165             if (operandA) {
 166                 return true;
 167             }
 168             //Operand B is deactivation operator
 169             if (operandB) {
 170                 return false;
 171             }
 172
 173             //When both operands are not met, keep current value
 174             return currentValue;
 175             break;
 176
 177         case LOGIC_CONDITION_EDGE:
 178             if (operandA && logicConditionStates[lcIndex].timeout == 0 && !(logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED)) {
 179                 if (operandB < 100) {
 180                     logicConditionStates[lcIndex].timeout = millis();
 181                 } else {
 182                     logicConditionStates[lcIndex].timeout = millis() + operandB;
 183                 }
 184                 logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
 185                 return true;
 186             } else if (logicConditionStates[lcIndex].timeout > 0) {
 187                 if (logicConditionStates[lcIndex].timeout < millis()) {
 188                     logicConditionStates[lcIndex].timeout = 0;
 189                 } else {
 190                     return true;
 191                 }
 192             }
 193
 194             if (!operandA) {
 195                 logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
 196             }
 197             return false;
 198             break;
 199
 200         case LOGIC_CONDITION_DELAY:
 201             if (operandA) {
 202                 if (logicConditionStates[lcIndex].timeout == 0) {
 203                     logicConditionStates[lcIndex].timeout = millis() + operandB;
 204                 } else if (millis() > logicConditionStates[lcIndex].timeout ) {
 205                     logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
 206                     return true;
 207                 } else if (logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED) {
 208                     return true;
 209                 }
 210             } else {
 211                 logicConditionStates[lcIndex].timeout = 0;
 212                 logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
 213             }
 214
 215             return false;
 216             break;
 217
 218         case LOGIC_CONDITION_TIMER:
 219             if ((logicConditionStates[lcIndex].timeout == 0) || (millis() > logicConditionStates[lcIndex].timeout && !currentValue)) {
 220                 logicConditionStates[lcIndex].timeout = millis() + operandA;
 221                 return true;
 222             } else if (millis() > logicConditionStates[lcIndex].timeout && currentValue) {
 223                 logicConditionStates[lcIndex].timeout = millis() + operandB;
 224                 return false;
 225             }
 226             return currentValue;
 227             break;
 228
 229         case LOGIC_CONDITION_DELTA:
 230             {
 231                 int difference = logicConditionStates[lcIndex].lastValue - operandA;
 232                 logicConditionStates[lcIndex].lastValue = operandA;
 233                 return ABS(difference) >= operandB;
 234             }
 235             break;
 236
 237         case LOGIC_CONDITION_GVAR_SET:
 238             gvSet(operandA, operandB);
 239             return operandB;
 240             break;
 241
 242         case LOGIC_CONDITION_GVAR_INC:
 243             temporaryValue = gvGet(operandA) + operandB;
 244             gvSet(operandA, temporaryValue);
 245             return temporaryValue;
 246             break;
 247
 248         case LOGIC_CONDITION_GVAR_DEC:
 249             temporaryValue = gvGet(operandA) - operandB;
 250             gvSet(operandA, temporaryValue);
 251             return temporaryValue;
 252             break;
 253
 254         case LOGIC_CONDITION_ADD:
 255             return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
 256             break;
 257
 258         case LOGIC_CONDITION_SUB:
 259             return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
 260             break;
 261
 262         case LOGIC_CONDITION_MUL:
 263             return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
 264             break;
 265
 266         case LOGIC_CONDITION_DIV:
 267             if (operandB != 0) {
 268                 return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
 269             } else {
 270                 return operandA;
 271             }
 272             break;
 273
 274         case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
 275             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
 276             return true;
 277             break;
 278
 279         case LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE:
 280             logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] = operandA;
 281             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE);
 282             return true;
 283             break;
 284
 285         case LOGIC_CONDITION_SWAP_ROLL_YAW:
 286             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW);
 287             return true;
 288             break;
 289
 290 #ifdef USE_MAG
 291         case LOGIC_CONDITION_RESET_MAG_CALIBRATION:
 292
 293             ENABLE_STATE(CALIBRATE_MAG);
 294             return true;
 295             break;
 296 #endif
 297         case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
 298 #if defined(USE_VTX_CONTROL)
 299 #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
 300             if (
 301                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
 302                 vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
 303             ) {
 304                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
 305                 vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
 306                 return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
 307             } else {
 308                 return false;
 309             }
 310             break;
 311 #else
 312             return false;
 313 #endif
 314
 315         case LOGIC_CONDITION_SET_VTX_BAND:
 316             if (
 317                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA &&
 318                 vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
 319             ) {
 320                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
 321                 vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
 322                 return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
 323             } else {
 324                 return false;
 325             }
 326             break;
 327         case LOGIC_CONDITION_SET_VTX_CHANNEL:
 328             if (
 329                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA &&
 330                 vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
 331             ) {
 332                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
 333                 vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
 334                 return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
 335             } else {
 336                 return false;
 337             }
 338             break;
 339 #endif
 340         case LOGIC_CONDITION_INVERT_ROLL:
 341             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
 342             return true;
 343             break;
 344
 345         case LOGIC_CONDITION_INVERT_PITCH:
 346             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
 347             return true;
 348             break;
 349
 350         case LOGIC_CONDITION_INVERT_YAW:
 351             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
 352             return true;
 353             break;
 354
 355         case LOGIC_CONDITION_OVERRIDE_THROTTLE:
 356             logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
 357             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
 358             return operandA;
 359             break;
 360
 361         case LOGIC_CONDITION_SET_OSD_LAYOUT:
 362             logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
 363             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
 364             return operandA;
 365             break;
 366
 367 #ifdef USE_I2C_IO_EXPANDER
 368         case LOGIC_CONDITION_PORT_SET:
 369             ioPortExpanderSet((uint8_t)operandA, (uint8_t)operandB);
 370             return operandB;
 371             break;
 372 #endif
 373
 374         case LOGIC_CONDITION_SIN:
 375             temporaryValue = (operandB == 0) ? 500 : operandB;
 376             return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 377             break;
 378
 379         case LOGIC_CONDITION_COS:
 380             temporaryValue = (operandB == 0) ? 500 : operandB;
 381             return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 382             break;
 383         break;
 384
 385         case LOGIC_CONDITION_TAN:
 386             temporaryValue = (operandB == 0) ? 500 : operandB;
 387             return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 388         break;
 389
 390         case LOGIC_CONDITION_MIN:
 391             return (operandA < operandB) ? operandA : operandB;
 392         break;
 393
 394         case LOGIC_CONDITION_MAX:
 395             return (operandA > operandB) ? operandA : operandB;
 396         break;
 397
 398         case LOGIC_CONDITION_MAP_INPUT:
 399             return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
 400         break;
 401
 402         case LOGIC_CONDITION_MAP_OUTPUT:
 403             return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
 404         break;
 405
 406         case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE:
 407             temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1);
 408             rcChannelOverrides[temporaryValue].active = true;
 409             rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX);
 410             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL);
 411             return true;
 412         break;
 413
 414         case LOGIC_CONDITION_SET_HEADING_TARGET:
 415             temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA)));
 416             updateHeadingHoldTarget(temporaryValue);
 417             return temporaryValue;
 418         break;
 419
 420         case LOGIC_CONDITION_MODULUS:
 421             if (operandB != 0) {
 422                 return constrain(operandA % operandB, INT32_MIN, INT32_MAX);
 423             } else {
 424                 return operandA;
 425             }
 426             break;
 427
 428         case LOGIC_CONDITION_SET_PROFILE:
 429             operandA--;
 430             if ( getConfigProfile() != operandA  && (operandA >= 0 && operandA < MAX_PROFILE_COUNT)) {
 431                 bool profileChanged = false;
 432                 if (setConfigProfile(operandA)) {
 433                     pidInit();
 434                     pidInitFilters();
 435                     schedulePidGainsUpdate();
 436                     navigationUsePIDs(); //set navigation pid gains
 437                     profileChanged = true;
 438                 }
 439                 return profileChanged;
 440             } else {
 441                 return false;
 442             }
 443             break;
 444
 445         case LOGIC_CONDITION_LOITER_OVERRIDE:
 446             logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
 447             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
 448             return true;
 449             break;
 450
 451         case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE:
 452             if (operandA >= 0 && operandA <= 2) {
 453
 454                 flightAxisOverride[operandA].angleTargetActive = true;
 455                 int target = DEGREES_TO_DECIDEGREES(operandB);
 456                 if (operandA == 0) {
 457                     //ROLL
 458                     target = constrain(target, -pidProfile()->max_angle_inclination[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
 459                 } else if (operandA == 1) {
 460                     //PITCH
 461                     target = constrain(target, -pidProfile()->max_angle_inclination[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
 462                 } else if (operandA == 2) {
 463                     //YAW
 464                     target = (constrain(target, 0, 3600));
 465                 }
 466                 flightAxisOverride[operandA].angleTarget = target;
 467                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
 468
 469                 return true;
 470             } else {
 471                 return false;
 472             }
 473             break;
 474
 475         case LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE:
 476             if (operandA >= 0 && operandA <= 2) {
 477                 flightAxisOverride[operandA].rateTargetActive = true;
 478                 flightAxisOverride[operandA].rateTarget = constrain(operandB, -2000, 2000);
 479                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
 480                 return true;
 481             } else {
 482                 return false;
 483             }
 484             break;
 485
 486 #ifdef USE_LED_STRIP
 487         case LOGIC_CONDITION_LED_PIN_PWM:
 488
 489             if (operandA >=0 && operandA <= 100) {
 490                 ledPinStartPWM((uint8_t)operandA);
 491             } else {
 492                 ledPinStopPWM();
 493             }
 494             return operandA;
 495             break;
 496 #endif
 497 #ifdef USE_GPS_FIX_ESTIMATION
 498         case LOGIC_CONDITION_DISABLE_GPS_FIX:
 499             if (operandA > 0) {
 500                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
 501             } else {
 502                 LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
 503             }
 504                 return true;
 505             break;
 506 #endif
 507
 508         default:
 509             return false;
 510             break;
 511     }
 512 }
 513
 514 void logicConditionProcess(uint8_t i) {
 515
 516     const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
 517
 518     if (logicConditions(i)->enabled && activatorValue && !cliMode) {
 519
 520         /*
 521          * Process condition only when latch flag is not set
 522          * Latched LCs can only go from OFF to ON, not the other way
 523          */
 524         if (!(logicConditionStates[i].flags & LOGIC_CONDITION_FLAG_LATCH)) {
 525             const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
 526             const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
 527             const int newValue = logicConditionCompute(
 528                 logicConditionStates[i].value,
 529                 logicConditions(i)->operation,
 530                 operandAValue,
 531                 operandBValue,
 532                 i
 533             );
 534
 535             logicConditionStates[i].value = newValue;
 536
 537             /*
 538              * if value evaluates as true, put a latch on logic condition
 539              */
 540             if (logicConditions(i)->flags & LOGIC_CONDITION_FLAG_LATCH && newValue) {
 541                 logicConditionStates[i].flags |= LOGIC_CONDITION_FLAG_LATCH;
 542             }
 543         }
 544     } else {
 545         logicConditionStates[i].value = false;
 546     }
 547 }
 548
 549 static int logicConditionGetWaypointOperandValue(int operand) {
 550
 551     switch (operand) {
 552         case LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP: // 0/1
 553             return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
 554             break;
 555
 556         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX:
 557             return NAV_Status.activeWpNumber;
 558             break;
 559
 560         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION:
 561             return NAV_Status.activeWpAction;
 562             break;
 563
 564         case LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION:
 565             {
 566                 uint8_t wpIndex = posControl.activeWaypointIndex + 1;
 567                 if ((wpIndex > 0) && (wpIndex < NAV_MAX_WAYPOINTS)) {
 568                     return posControl.waypointList[wpIndex].action;
 569                 }
 570                 return false;
 571             }
 572             break;
 573
 574         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE:
 575             {
 576                 uint32_t distance = 0;
 577                 if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
 578                     fpVector3_t poi;
 579                     gpsLocation_t wp;
 580                     wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat;
 581                     wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon;
 582                     wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt;
 583                     geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
 584
 585                     distance = calculateDistanceToDestination(&poi) / 100;
 586                 }
 587
 588                 return distance;
 589             }
 590             break;
 591
 592         case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT:
 593             {
 594                 uint32_t distance = 0;
 595                 if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) {
 596                     fpVector3_t poi;
 597                     gpsLocation_t wp;
 598                     wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat;
 599                     wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon;
 600                     wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt;
 601                     geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
 602
 603                     distance = calculateDistanceToDestination(&poi) / 100;
 604                 }
 605
 606                 return distance;
 607             }
 608             break;
 609
 610         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
 611             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
 612             break;
 613
 614         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION:
 615             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0;
 616             break;
 617
 618         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION:
 619             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0;
 620             break;
 621
 622         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION:
 623             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0;
 624             break;
 625
 626         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP:
 627             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1);
 628             break;
 629
 630         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP:
 631             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2);
 632             break;
 633
 634         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP:
 635             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3);
 636             break;
 637
 638         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP:
 639             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4);
 640             break;
 641
 642         default:
 643             return 0;
 644             break;
 645     }
 646 }
 647
 648 static int logicConditionGetFlightOperandValue(int operand) {
 649
 650     switch (operand) {
 651
 652         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
 653             return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
 654             break;
 655
 656         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
 657             return constrain(GPS_distanceToHome, 0, INT16_MAX);
 658             break;
 659
 660         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
 661             return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
 662             break;
 663
 664         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
 665             return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
 666             break;
 667
 668         case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
 669             return getBatteryVoltage();
 670             break;
 671
 672         case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
 673             return getBatteryAverageCellVoltage();
 674             break;
 675
 676         case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS:
 677             return getBatteryCellCount();
 678             break;
 679
 680         case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
 681             return getAmperage();
 682             break;
 683
 684         case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
 685             return getMAhDrawn();
 686             break;
 687
 688         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
 689 #ifdef USE_GPS_FIX_ESTIMATION
 690             if ( STATE(GPS_ESTIMATED_FIX) ){
 691                 return gpsSol.numSat; //99
 692             } else
 693 #endif
 694             if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
 695                 return 0;
 696             } else {
 697                 return gpsSol.numSat;
 698             }
 699             break;
 700
 701         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
 702             return STATE(GPS_FIX) ? 1 : 0;
 703             break;
 704
 705         case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
 706             return gpsSol.groundSpeed;
 707             break;
 708
 709         //FIXME align with osdGet3DSpeed
 710         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
 711             return osdGet3DSpeed();
 712             break;
 713
 714         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
 715         #ifdef USE_PITOT
 716             return constrain(getAirspeedEstimate(), 0, INT16_MAX);
 717         #else
 718             return false;
 719         #endif
 720             break;
 721
 722         case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
 723             return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX);
 724             break;
 725
 726         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
 727             return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX);
 728             break;
 729
 730         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
 731             return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 732             break;
 733
 734         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg
 735             return constrain(attitude.values.roll / 10, -180, 180);
 736             break;
 737
 738         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg
 739             return constrain(attitude.values.pitch / 10, -180, 180);
 740             break;
 741
 742         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg
 743             return constrain(attitude.values.yaw / 10, 0, 360);
 744             break;
 745
 746         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
 747             return ARMING_FLAG(ARMED) ? 1 : 0;
 748             break;
 749
 750         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
 751             return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
 752             break;
 753
 754         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
 755             return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
 756             break;
 757
 758         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
 759             return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
 760             break;
 761
 762         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
 763             return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
 764             break;
 765
 766         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
 767             return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
 768             break;
 769
 770         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
 771 #ifdef USE_FW_AUTOLAND
 772             return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0;
 773 #else
 774             return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
 775 #endif
 776
 777             break;
 778
 779         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
 780             return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
 781             break;
 782
 783         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
 784             return axisPID[YAW];
 785             break;
 786
 787         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
 788             return axisPID[ROLL];
 789             break;
 790
 791         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
 792             return axisPID[PITCH];
 793             break;
 794
 795         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
 796             return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX);
 797             break;
 798
 799         case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
 800         #ifdef USE_SERIALRX_CRSF
 801             return rxLinkStatistics.uplinkLQ;
 802         #else
 803             return 0;
 804         #endif
 805             break;
 806
 807         case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
 808         #ifdef USE_SERIALRX_CRSF
 809             return rxLinkStatistics.uplinkSNR;
 810         #else
 811             return 0;
 812         #endif
 813             break;
 814
 815         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
 816             return getConfigProfile() + 1;
 817             break;
 818
 819         case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
 820             return getConfigBatteryProfile() + 1;
 821             break;
 822
 823         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
 824             return currentMixerProfileIndex + 1;
 825             break;
 826
 827         case LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE: //0,1
 828             return isMixerTransitionMixing ? 1 : 0;
 829             break;
 830
 831         case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
 832             return getLoiterRadius(navConfig()->fw.loiter_radius);
 833             break;
 834
 835         case LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS:
 836             return getFlownLoiterRadius();
 837             break;
 838
 839         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
 840             return isEstimatedAglTrusted();
 841             break;
 842
 843         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
 844             return getEstimatedAglPosition();
 845             break;
 846
 847         case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
 848             return rangefinderGetLatestRawAltitude();
 849             break;
 850 #ifdef USE_FW_AUTOLAND
 851         case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
 852             return posControl.fwLandState.landState;
 853             break;
 854 #endif
 855         default:
 856             return 0;
 857             break;
 858     }
 859 }
 860
 861 static int logicConditionGetFlightModeOperandValue(int operand) {
 862
 863     switch (operand) {
 864
 865         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE:
 866             return (bool) FLIGHT_MODE(FAILSAFE_MODE);
 867             break;
 868
 869         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL:
 870             return (bool) FLIGHT_MODE(MANUAL_MODE);
 871             break;
 872
 873         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH:
 874             return (bool) FLIGHT_MODE(NAV_RTH_MODE);
 875             break;
 876
 877         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD:
 878             return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
 879             break;
 880
 881         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION:
 882             return (bool) FLIGHT_MODE(NAV_WP_MODE);
 883             break;
 884
 885         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
 886             return ((bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !(bool)FLIGHT_MODE(NAV_ALTHOLD_MODE));
 887             break;
 888
 889         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
 890             return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
 891             break;
 892
 893         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
 894             return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE);
 895             break;
 896
 897         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE:
 898             return (bool) FLIGHT_MODE(ANGLE_MODE);
 899             break;
 900
 901         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON:
 902             return (bool) FLIGHT_MODE(HORIZON_MODE);
 903             break;
 904
 905         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD:
 906             return (bool) FLIGHT_MODE(ANGLEHOLD_MODE);
 907             break;
 908
 909         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
 910             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
 911             break;
 912
 913         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
 914             return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(ANGLEHOLD_MODE) ||
 915                      (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) ||
 916                      (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
 917             break;
 918
 919         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
 920             return IS_RC_MODE_ACTIVE(BOXUSER1);
 921             break;
 922
 923         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2:
 924             return IS_RC_MODE_ACTIVE(BOXUSER2);
 925             break;
 926
 927         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3:
 928             return IS_RC_MODE_ACTIVE(BOXUSER3);
 929             break;
 930
 931         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4:
 932             return IS_RC_MODE_ACTIVE(BOXUSER4);
 933             break;
 934
 935         default:
 936             return 0;
 937             break;
 938     }
 939 }
 940
 941 int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
 942     int retVal = 0;
 943
 944     switch (type) {
 945
 946         case LOGIC_CONDITION_OPERAND_TYPE_VALUE:
 947             retVal = operand;
 948             break;
 949
 950         case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
 951             //Extract RC channel raw value
 952             if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
 953                 retVal = rxGetChannelValue(operand - 1);
 954             }
 955             break;
 956
 957         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
 958             retVal = logicConditionGetFlightOperandValue(operand);
 959             break;
 960
 961         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE:
 962             retVal = logicConditionGetFlightModeOperandValue(operand);
 963             break;
 964
 965         case LOGIC_CONDITION_OPERAND_TYPE_LC:
 966             if (operand >= 0 && operand < MAX_LOGIC_CONDITIONS) {
 967                 retVal = logicConditionGetValue(operand);
 968             }
 969             break;
 970
 971         case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
 972             if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
 973                 retVal = gvGet(operand);
 974             }
 975             break;
 976
 977         case LOGIC_CONDITION_OPERAND_TYPE_PID:
 978             if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
 979                 retVal = programmingPidGetOutput(operand);
 980             }
 981             break;
 982
 983         case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
 984             retVal = logicConditionGetWaypointOperandValue(operand);
 985             break;
 986
 987         default:
 988             break;
 989     }
 990
 991     return retVal;
 992 }
 993
 994 /*
 995  * conditionId == -1 is always evaluated as true
 996  */
 997 int logicConditionGetValue(int8_t conditionId) {
 998     if (conditionId >= 0) {
 999         return logicConditionStates[conditionId].value;
1000     } else {
1001         return true;
1002     }
1003 }
1004
1005 void logicConditionUpdateTask(timeUs_t currentTimeUs) {
1006     UNUSED(currentTimeUs);
1007
1008     //Disable all flags
1009     logicConditionsGlobalFlags = 0;
1010
1011     for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
1012         rcChannelOverrides[i].active = false;
1013     }
1014
1015     for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
1016         flightAxisOverride[i].rateTargetActive = false;
1017         flightAxisOverride[i].angleTargetActive = false;
1018     }
1019
1020     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
1021         logicConditionProcess(i);
1022     }
1023
1024 #ifdef USE_I2C_IO_EXPANDER
1025     ioPortExpanderSync();
1026 #endif
1027 }
1028
1029 void logicConditionReset(void) {
1030     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
1031         logicConditionStates[i].value = 0;
1032         logicConditionStates[i].lastValue = 0;
1033         logicConditionStates[i].flags = 0;
1034         logicConditionStates[i].timeout = 0;
1035     }
1036 }
1037
1038 float NOINLINE getThrottleScale(float globalThrottleScale) {
1039     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE)) {
1040         return constrainf(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] / 100.0f, 0.0f, 1.0f);
1041     } else {
1042         return globalThrottleScale;
1043     }
1044 }
1045
1046 int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
1047     int16_t outputValue = command[axis];
1048
1049     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
1050         outputValue = command[FD_YAW];
1051     } else if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
1052         outputValue = command[FD_ROLL];
1053     }
1054
1055     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
1056         outputValue *= -1;
1057     }
1058     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
1059         outputValue *= -1;
1060     }
1061     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
1062         outputValue *= -1;
1063     }
1064
1065     return outputValue;
1066 }
1067
1068 int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
1069     if (rcChannelOverrides[channel].active) {
1070         return rcChannelOverrides[channel].value;
1071     } else {
1072         return originalValue;
1073     }
1074 }
1075
1076 uint32_t getLoiterRadius(uint32_t loiterRadius) {
1077 #ifdef USE_PROGRAMMING_FRAMEWORK
1078     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
1079         !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
1080         return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
1081     } else {
1082         return loiterRadius;
1083     }
1084 #else
1085     return loiterRadius;
1086 #endif
1087 }
1088
1089 float getFlightAxisAngleOverride(uint8_t axis, float angle) {
1090     if (flightAxisOverride[axis].angleTargetActive) {
1091         return flightAxisOverride[axis].angleTarget;
1092     } else {
1093         return angle;
1094     }
1095 }
1096
1097 float getFlightAxisRateOverride(uint8_t axis, float rate) {
1098     if (flightAxisOverride[axis].rateTargetActive) {
1099         return flightAxisOverride[axis].rateTarget;
1100     } else {
1101         return rate;
1102     }
1103 }
1104
1105 bool isFlightAxisAngleOverrideActive(uint8_t axis) {
1106     if (flightAxisOverride[axis].angleTargetActive) {
1107         return true;
1108     } else {
1109         return false;
1110     }
1111 }
1112
1113 bool isFlightAxisRateOverrideActive(uint8_t axis) {
1114     if (flightAxisOverride[axis].rateTargetActive) {
1115         return true;
1116     } else {
1117         return false;
1118     }
1119 }