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
 298 #if defined(USE_VTX_CONTROL)
 299         case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
 300         {
 301             uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
 302             if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
 303                 newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
 304                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower;
 305                 if (newPower != vtxSettingsConfig()->power) {
 306                     vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere
 307                 }
 308                 vtxSettingsConfigMutable()->power = newPower;
 309                 return newPower;
 310             }
 311             return false;
 312             break;
 313         }
 314         case LOGIC_CONDITION_SET_VTX_BAND:
 315         {
 316             uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
 317             if ((newBand != operandA  || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
 318                 newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount);
 319                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand;
 320                 if (newBand != vtxSettingsConfig()->band) {
 321                     vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel);
 322                 }
 323                 vtxSettingsConfigMutable()->band = newBand;
 324                 return newBand;
 325             }
 326             return false;
 327             break;
 328         }
 329         case LOGIC_CONDITION_SET_VTX_CHANNEL:
 330         {
 331             uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
 332             if ((newChannel != operandA  || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
 333                 newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount);
 334                 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel;
 335                 if (newChannel != vtxSettingsConfig()->channel) {
 336                     vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel);
 337                 }
 338                 vtxSettingsConfigMutable()->channel = newChannel;
 339                 return newChannel;
 340             }
 341             return false;
 342             break;
 343         }
 344 #endif
 345         case LOGIC_CONDITION_INVERT_ROLL:
 346             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
 347             return true;
 348             break;
 349
 350         case LOGIC_CONDITION_INVERT_PITCH:
 351             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
 352             return true;
 353             break;
 354
 355         case LOGIC_CONDITION_INVERT_YAW:
 356             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
 357             return true;
 358             break;
 359
 360         case LOGIC_CONDITION_OVERRIDE_THROTTLE:
 361             logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
 362             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
 363             return operandA;
 364             break;
 365
 366         case LOGIC_CONDITION_SET_OSD_LAYOUT:
 367             logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
 368             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
 369             return operandA;
 370             break;
 371
 372 #ifdef USE_I2C_IO_EXPANDER
 373         case LOGIC_CONDITION_PORT_SET:
 374             ioPortExpanderSet((uint8_t)operandA, (uint8_t)operandB);
 375             return operandB;
 376             break;
 377 #endif
 378
 379         case LOGIC_CONDITION_SIN:
 380             temporaryValue = (operandB == 0) ? 500 : operandB;
 381             return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 382             break;
 383
 384         case LOGIC_CONDITION_COS:
 385             temporaryValue = (operandB == 0) ? 500 : operandB;
 386             return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 387             break;
 388         break;
 389
 390         case LOGIC_CONDITION_TAN:
 391             temporaryValue = (operandB == 0) ? 500 : operandB;
 392             return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
 393         break;
 394
 395         case LOGIC_CONDITION_MIN:
 396             return (operandA < operandB) ? operandA : operandB;
 397         break;
 398
 399         case LOGIC_CONDITION_MAX:
 400             return (operandA > operandB) ? operandA : operandB;
 401         break;
 402
 403         case LOGIC_CONDITION_MAP_INPUT:
 404             return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
 405         break;
 406
 407         case LOGIC_CONDITION_MAP_OUTPUT:
 408             return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
 409         break;
 410
 411         case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE:
 412             temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1);
 413             rcChannelOverrides[temporaryValue].active = true;
 414             rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX);
 415             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL);
 416             return true;
 417         break;
 418
 419         case LOGIC_CONDITION_SET_HEADING_TARGET:
 420             temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA)));
 421             updateHeadingHoldTarget(temporaryValue);
 422             return temporaryValue;
 423         break;
 424
 425         case LOGIC_CONDITION_MODULUS:
 426             if (operandB != 0) {
 427                 return constrain(operandA % operandB, INT32_MIN, INT32_MAX);
 428             } else {
 429                 return operandA;
 430             }
 431             break;
 432
 433         case LOGIC_CONDITION_SET_PROFILE:
 434             operandA--;
 435             if ( getConfigProfile() != operandA  && (operandA >= 0 && operandA < MAX_PROFILE_COUNT)) {
 436                 bool profileChanged = false;
 437                 if (setConfigProfile(operandA)) {
 438                     pidInit();
 439                     pidInitFilters();
 440                     schedulePidGainsUpdate();
 441                     navigationUsePIDs(); //set navigation pid gains
 442                     profileChanged = true;
 443                 }
 444                 return profileChanged;
 445             } else {
 446                 return false;
 447             }
 448             break;
 449
 450         case LOGIC_CONDITION_LOITER_OVERRIDE:
 451             logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
 452             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
 453             return true;
 454             break;
 455
 456         case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE:
 457             if (operandA >= 0 && operandA <= 2) {
 458
 459                 flightAxisOverride[operandA].angleTargetActive = true;
 460                 int target = DEGREES_TO_DECIDEGREES(operandB);
 461                 if (operandA == 0) {
 462                     //ROLL
 463                     target = constrain(target, -pidProfile()->max_angle_inclination[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
 464                 } else if (operandA == 1) {
 465                     //PITCH
 466                     target = constrain(target, -pidProfile()->max_angle_inclination[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
 467                 } else if (operandA == 2) {
 468                     //YAW
 469                     target = (constrain(target, 0, 3600));
 470                 }
 471                 flightAxisOverride[operandA].angleTarget = target;
 472                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
 473
 474                 return true;
 475             } else {
 476                 return false;
 477             }
 478             break;
 479
 480         case LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE:
 481             if (operandA >= 0 && operandA <= 2) {
 482                 flightAxisOverride[operandA].rateTargetActive = true;
 483                 flightAxisOverride[operandA].rateTarget = constrain(operandB, -2000, 2000);
 484                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS);
 485                 return true;
 486             } else {
 487                 return false;
 488             }
 489             break;
 490
 491 #ifdef USE_LED_STRIP
 492         case LOGIC_CONDITION_LED_PIN_PWM:
 493
 494             if (operandA >=0 && operandA <= 100) {
 495                 ledPinStartPWM((uint8_t)operandA);
 496             } else {
 497                 ledPinStopPWM();
 498             }
 499             return operandA;
 500             break;
 501 #endif
 502 #ifdef USE_GPS_FIX_ESTIMATION
 503         case LOGIC_CONDITION_DISABLE_GPS_FIX:
 504             if (operandA > 0) {
 505                 LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
 506             } else {
 507                 LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
 508             }
 509                 return true;
 510             break;
 511 #endif
 512
 513         default:
 514             return false;
 515             break;
 516     }
 517 }
 518
 519 void logicConditionProcess(uint8_t i) {
 520
 521     const int32_t activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
 522
 523     if (logicConditions(i)->enabled && activatorValue && !cliMode) {
 524
 525         /*
 526          * Process condition only when latch flag is not set
 527          * Latched LCs can only go from OFF to ON, not the other way
 528          */
 529         if (!(logicConditionStates[i].flags & LOGIC_CONDITION_FLAG_LATCH)) {
 530             const int32_t operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
 531             const int32_t operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
 532             const int32_t newValue = logicConditionCompute(
 533                 logicConditionStates[i].value,
 534                 logicConditions(i)->operation,
 535                 operandAValue,
 536                 operandBValue,
 537                 i
 538             );
 539
 540             logicConditionStates[i].value = newValue;
 541
 542             /*
 543              * if value evaluates as true, put a latch on logic condition
 544              */
 545             if (logicConditions(i)->flags & LOGIC_CONDITION_FLAG_LATCH && newValue) {
 546                 logicConditionStates[i].flags |= LOGIC_CONDITION_FLAG_LATCH;
 547             }
 548         }
 549     } else {
 550         logicConditionStates[i].value = false;
 551     }
 552 }
 553
 554 static int logicConditionGetWaypointOperandValue(int operand) {
 555
 556     switch (operand) {
 557         case LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP: // 0/1
 558             return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
 559             break;
 560
 561         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX:
 562             return NAV_Status.activeWpNumber;
 563             break;
 564
 565         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION:
 566             return NAV_Status.activeWpAction;
 567             break;
 568
 569         case LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION:
 570             {
 571                 uint8_t wpIndex = posControl.activeWaypointIndex + 1;
 572                 if ((wpIndex > 0) && (wpIndex < NAV_MAX_WAYPOINTS)) {
 573                     return posControl.waypointList[wpIndex].action;
 574                 }
 575                 return false;
 576             }
 577             break;
 578
 579         case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE:
 580             {
 581                 uint32_t distance = 0;
 582                 if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
 583                     fpVector3_t poi;
 584                     gpsLocation_t wp;
 585                     wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat;
 586                     wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon;
 587                     wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt;
 588                     geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
 589
 590                     distance = calculateDistanceToDestination(&poi) / 100;
 591                 }
 592
 593                 return distance;
 594             }
 595             break;
 596
 597         case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT:
 598             {
 599                 uint32_t distance = 0;
 600                 if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) {
 601                     fpVector3_t poi;
 602                     gpsLocation_t wp;
 603                     wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat;
 604                     wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon;
 605                     wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt;
 606                     geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
 607
 608                     distance = calculateDistanceToDestination(&poi) / 100;
 609                 }
 610
 611                 return distance;
 612             }
 613             break;
 614
 615         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
 616             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
 617             break;
 618
 619         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION:
 620             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0;
 621             break;
 622
 623         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION:
 624             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0;
 625             break;
 626
 627         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION:
 628             return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0;
 629             break;
 630
 631         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP:
 632             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1);
 633             break;
 634
 635         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP:
 636             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2);
 637             break;
 638
 639         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP:
 640             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3);
 641             break;
 642
 643         case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP:
 644             return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4);
 645             break;
 646
 647         default:
 648             return 0;
 649             break;
 650     }
 651 }
 652
 653 static int logicConditionGetFlightOperandValue(int operand) {
 654
 655     switch (operand) {
 656
 657         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
 658             return constrain((uint32_t)getFlightTime(), 0, INT32_MAX);
 659             break;
 660
 661         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
 662             return constrain(GPS_distanceToHome, 0, INT32_MAX);
 663             break;
 664
 665         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
 666             return constrain(getTotalTravelDistance() / 100, 0, INT32_MAX);
 667             break;
 668
 669         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
 670             return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
 671             break;
 672
 673         case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
 674             return getBatteryVoltage();
 675             break;
 676
 677         case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
 678             return getBatteryAverageCellVoltage();
 679             break;
 680
 681         case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS:
 682             return getBatteryCellCount();
 683             break;
 684
 685         case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
 686             return getAmperage();
 687             break;
 688
 689         case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
 690             return getMAhDrawn();
 691             break;
 692
 693         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
 694 #ifdef USE_GPS_FIX_ESTIMATION
 695             if ( STATE(GPS_ESTIMATED_FIX) ){
 696                 return gpsSol.numSat; //99
 697             } else
 698 #endif
 699             if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
 700                 return 0;
 701             } else {
 702                 return gpsSol.numSat;
 703             }
 704             break;
 705
 706         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
 707             return STATE(GPS_FIX) ? 1 : 0;
 708             break;
 709
 710         case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
 711             return gpsSol.groundSpeed;
 712             break;
 713
 714         //FIXME align with osdGet3DSpeed
 715         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
 716             return osdGet3DSpeed();
 717             break;
 718
 719         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
 720         #ifdef USE_PITOT
 721             return constrain(getAirspeedEstimate(), 0, INT32_MAX);
 722         #else
 723             return false;
 724         #endif
 725             break;
 726
 727         case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
 728             return constrain(getEstimatedActualPosition(Z), INT32_MIN, INT32_MAX);
 729             break;
 730
 731         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
 732             return constrain(getEstimatedActualVelocity(Z), INT32_MIN, INT32_MAX);
 733             break;
 734
 735         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
 736             return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 737             break;
 738
 739         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg
 740             return constrain(attitude.values.roll / 10, -180, 180);
 741             break;
 742
 743         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg
 744             return constrain(attitude.values.pitch / 10, -180, 180);
 745             break;
 746
 747         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg
 748             return constrain(attitude.values.yaw / 10, 0, 360);
 749             break;
 750
 751         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
 752             return ARMING_FLAG(ARMED) ? 1 : 0;
 753             break;
 754
 755         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
 756             return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
 757             break;
 758
 759         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
 760             return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
 761             break;
 762
 763         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
 764             return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
 765             break;
 766
 767         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
 768             return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
 769             break;
 770
 771         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
 772             return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
 773             break;
 774
 775         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
 776 #ifdef USE_FW_AUTOLAND
 777             return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0;
 778 #else
 779             return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
 780 #endif
 781
 782             break;
 783
 784         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
 785             return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
 786             break;
 787
 788         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
 789             return axisPID[YAW];
 790             break;
 791
 792         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
 793             return axisPID[ROLL];
 794             break;
 795
 796         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
 797             return axisPID[PITCH];
 798             break;
 799
 800         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m
 801             return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX);
 802             break;
 803
 804         case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK:
 805 #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
 806             return rxLinkStatistics.uplinkLQ;
 807 #else
 808             return 0;
 809 #endif
 810             break;
 811
 812         case LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM:
 813 #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
 814             return rxLinkStatistics.uplinkRSSI;
 815 #else
 816             return 0;
 817 #endif
 818             break;
 819
 820 case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK:
 821 #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
 822             return rxLinkStatistics.downlinkLQ;
 823 #else
 824             return 0;
 825 #endif
 826             break;
 827
 828         case LOGIC_CONDITION_OPERAND_FLIGHT_SNR:
 829 #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
 830             return rxLinkStatistics.uplinkSNR;
 831 #else
 832             return 0;
 833 #endif
 834             break;
 835
 836         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
 837             return getConfigProfile() + 1;
 838             break;
 839
 840         case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
 841             return getConfigBatteryProfile() + 1;
 842             break;
 843
 844         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
 845             return currentMixerProfileIndex + 1;
 846             break;
 847
 848         case LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE: //0,1
 849             return isMixerTransitionMixing ? 1 : 0;
 850             break;
 851
 852         case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
 853             return getLoiterRadius(navConfig()->fw.loiter_radius);
 854             break;
 855
 856         case LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS:
 857             return getFlownLoiterRadius();
 858             break;
 859
 860         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
 861             return isEstimatedAglTrusted();
 862             break;
 863
 864         case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
 865             return getEstimatedAglPosition();
 866             break;
 867
 868         case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
 869             return rangefinderGetLatestRawAltitude();
 870             break;
 871 #ifdef USE_FW_AUTOLAND
 872         case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
 873             return posControl.fwLandState.landState;
 874             break;
 875 #endif
 876         default:
 877             return 0;
 878             break;
 879     }
 880 }
 881
 882 static int logicConditionGetFlightModeOperandValue(int operand) {
 883
 884     switch (operand) {
 885
 886         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE:
 887             return (bool) FLIGHT_MODE(FAILSAFE_MODE);
 888             break;
 889
 890         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL:
 891             return (bool) FLIGHT_MODE(MANUAL_MODE);
 892             break;
 893
 894         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH:
 895             return (bool) FLIGHT_MODE(NAV_RTH_MODE);
 896             break;
 897
 898         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD:
 899             return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
 900             break;
 901
 902         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION:
 903             return (bool) FLIGHT_MODE(NAV_WP_MODE);
 904             break;
 905
 906         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
 907             return ((bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !(bool)FLIGHT_MODE(NAV_ALTHOLD_MODE));
 908             break;
 909
 910         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
 911             return (bool) (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
 912             break;
 913
 914         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
 915             return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE);
 916             break;
 917
 918         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE:
 919             return (bool) FLIGHT_MODE(ANGLE_MODE);
 920             break;
 921
 922         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON:
 923             return (bool) FLIGHT_MODE(HORIZON_MODE);
 924             break;
 925
 926         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD:
 927             return (bool) FLIGHT_MODE(ANGLEHOLD_MODE);
 928             break;
 929
 930         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
 931             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
 932             break;
 933
 934         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO:
 935             return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(ANGLEHOLD_MODE) ||
 936                      (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) ||
 937                      (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false);
 938             break;
 939
 940         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1:
 941             return IS_RC_MODE_ACTIVE(BOXUSER1);
 942             break;
 943
 944         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2:
 945             return IS_RC_MODE_ACTIVE(BOXUSER2);
 946             break;
 947
 948         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3:
 949             return IS_RC_MODE_ACTIVE(BOXUSER3);
 950             break;
 951
 952         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4:
 953             return IS_RC_MODE_ACTIVE(BOXUSER4);
 954             break;
 955
 956         default:
 957             return 0;
 958             break;
 959     }
 960 }
 961
 962 int32_t logicConditionGetOperandValue(logicOperandType_e type, int operand) {
 963     int32_t retVal = 0;
 964
 965     switch (type) {
 966
 967         case LOGIC_CONDITION_OPERAND_TYPE_VALUE:
 968             retVal = operand;
 969             break;
 970
 971         case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
 972             //Extract RC channel raw value
 973             if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
 974                 retVal = rxGetChannelValue(operand - 1);
 975             }
 976             break;
 977
 978         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
 979             retVal = logicConditionGetFlightOperandValue(operand);
 980             break;
 981
 982         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE:
 983             retVal = logicConditionGetFlightModeOperandValue(operand);
 984             break;
 985
 986         case LOGIC_CONDITION_OPERAND_TYPE_LC:
 987             if (operand >= 0 && operand < MAX_LOGIC_CONDITIONS) {
 988                 retVal = logicConditionGetValue(operand);
 989             }
 990             break;
 991
 992         case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
 993             if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
 994                 retVal = gvGet(operand);
 995             }
 996             break;
 997
 998         case LOGIC_CONDITION_OPERAND_TYPE_PID:
 999             if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
1000                 retVal = programmingPidGetOutput(operand);
1001             }
1002             break;
1003
1004         case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
1005             retVal = logicConditionGetWaypointOperandValue(operand);
1006             break;
1007
1008         default:
1009             break;
1010     }
1011
1012     return retVal;
1013 }
1014
1015 /*
1016  * conditionId == -1 is always evaluated as true
1017  */
1018 int32_t logicConditionGetValue(int8_t conditionId) {
1019     if (conditionId >= 0) {
1020         return logicConditionStates[conditionId].value;
1021     } else {
1022         return true;
1023     }
1024 }
1025
1026 void logicConditionUpdateTask(timeUs_t currentTimeUs) {
1027     UNUSED(currentTimeUs);
1028
1029     //Disable all flags
1030     logicConditionsGlobalFlags = 0;
1031
1032     for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
1033         rcChannelOverrides[i].active = false;
1034     }
1035
1036     for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
1037         flightAxisOverride[i].rateTargetActive = false;
1038         flightAxisOverride[i].angleTargetActive = false;
1039     }
1040
1041     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
1042         logicConditionProcess(i);
1043     }
1044
1045 #ifdef USE_I2C_IO_EXPANDER
1046     ioPortExpanderSync();
1047 #endif
1048 }
1049
1050 void logicConditionReset(void) {
1051     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
1052         logicConditionStates[i].value = 0;
1053         logicConditionStates[i].lastValue = 0;
1054         logicConditionStates[i].flags = 0;
1055         logicConditionStates[i].timeout = 0;
1056     }
1057 }
1058
1059 float NOINLINE getThrottleScale(float globalThrottleScale) {
1060     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE)) {
1061         return constrainf(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE] / 100.0f, 0.0f, 1.0f);
1062     } else {
1063         return globalThrottleScale;
1064     }
1065 }
1066
1067 int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
1068     int16_t outputValue = command[axis];
1069
1070     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
1071         outputValue = command[FD_YAW];
1072     } else if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_YAW) {
1073         outputValue = command[FD_ROLL];
1074     }
1075
1076     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL) && axis == FD_ROLL) {
1077         outputValue *= -1;
1078     }
1079     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH) && axis == FD_PITCH) {
1080         outputValue *= -1;
1081     }
1082     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW) && axis == FD_YAW) {
1083         outputValue *= -1;
1084     }
1085
1086     return outputValue;
1087 }
1088
1089 int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
1090     if (rcChannelOverrides[channel].active) {
1091         return rcChannelOverrides[channel].value;
1092     } else {
1093         return originalValue;
1094     }
1095 }
1096
1097 uint32_t getLoiterRadius(uint32_t loiterRadius) {
1098 #ifdef USE_PROGRAMMING_FRAMEWORK
1099     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
1100         !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
1101         return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
1102     } else {
1103         return loiterRadius;
1104     }
1105 #else
1106     return loiterRadius;
1107 #endif
1108 }
1109
1110 float getFlightAxisAngleOverride(uint8_t axis, float angle) {
1111     if (flightAxisOverride[axis].angleTargetActive) {
1112         return flightAxisOverride[axis].angleTarget;
1113     } else {
1114         return angle;
1115     }
1116 }
1117
1118 float getFlightAxisRateOverride(uint8_t axis, float rate) {
1119     if (flightAxisOverride[axis].rateTargetActive) {
1120         return flightAxisOverride[axis].rateTarget;
1121     } else {
1122         return rate;
1123     }
1124 }
1125
1126 bool isFlightAxisAngleOverrideActive(uint8_t axis) {
1127     if (flightAxisOverride[axis].angleTargetActive) {
1128         return true;
1129     } else {
1130         return false;
1131     }
1132 }
1133
1134 bool isFlightAxisRateOverrideActive(uint8_t axis) {
1135     if (flightAxisOverride[axis].rateTargetActive) {
1136         return true;
1137     } else {
1138         return false;
1139     }
1140 }