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 "common/utils.h"
  34 #include "rx/rx.h"
  35 #include "common/maths.h"
  36 #include "fc/fc_core.h"
  37 #include "fc/rc_controls.h"
  38 #include "fc/runtime_config.h"
  39 #include "navigation/navigation.h"
  40 #include "sensors/battery.h"
  41 #include "sensors/pitotmeter.h"
  42 #include "flight/imu.h"
  43 #include "flight/pid.h"
  44
  45 #include "navigation/navigation.h"
  46 #include "navigation/navigation_private.h"
  47
  48 PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 1);
  49
  50 void pgResetFn_logicConditions(logicCondition_t *instance)
  51 {
  52     for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
  53         RESET_CONFIG(logicCondition_t, &instance[i],
  54             .enabled = 0,
  55             .activatorId = -1,
  56             .operation = 0,
  57             .operandA = {
  58                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  59                 .value = 0
  60             },
  61             .operandB = {
  62                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  63                 .value = 0
  64             },
  65             .flags = 0
  66         );
  67     }
  68 }
  69
  70 logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
  71
  72 static int logicConditionCompute(
  73     int currentVaue,
  74     logicOperation_e operation,
  75     int operandA,
  76     int operandB
  77 ) {
  78     int temporaryValue;
  79     switch (operation) {
  80
  81         case LOGIC_CONDITION_TRUE:
  82             return true;
  83             break;
  84
  85         case LOGIC_CONDITION_EQUAL:
  86             return operandA == operandB;
  87             break;
  88
  89         case LOGIC_CONDITION_GREATER_THAN:
  90             return operandA > operandB;
  91             break;
  92
  93         case LOGIC_CONDITION_LOWER_THAN:
  94             return operandA < operandB;
  95             break;
  96
  97         case LOGIC_CONDITION_LOW:
  98             return operandA < 1333;
  99             break;
 100
 101         case LOGIC_CONDITION_MID:
 102             return operandA >= 1333 && operandA <= 1666;
 103             break;
 104
 105         case LOGIC_CONDITION_HIGH:
 106             return operandA > 1666;
 107             break;
 108
 109         case LOGIC_CONDITION_AND:
 110             return (operandA && operandB);
 111             break;
 112
 113         case LOGIC_CONDITION_OR:
 114             return (operandA || operandB);
 115             break;
 116
 117         case LOGIC_CONDITION_XOR:
 118             return (operandA != operandB);
 119             break;
 120
 121         case LOGIC_CONDITION_NAND:
 122             return !(operandA && operandB);
 123             break;
 124
 125         case LOGIC_CONDITION_NOR:
 126             return !(operandA || operandB);
 127             break;
 128
 129         case LOGIC_CONDITION_NOT:
 130             return !operandA;
 131             break;
 132
 133         case LOGIC_CONDITION_STICKY:
 134             // Operand A is activation operator
 135             if (operandA) {
 136                 return true;
 137             }
 138             //Operand B is deactivation operator
 139             if (operandB) {
 140                 return false;
 141             }
 142
 143             //When both operands are not met, keep current value
 144             return currentVaue;
 145             break;
 146
 147         case LOGIC_CONDITION_GVAR_SET:
 148             gvSet(operandA, operandB);
 149             return operandB;
 150             break;
 151
 152         case LOGIC_CONDITION_GVAR_INC:
 153             temporaryValue = gvGet(operandA) + operandB;
 154             gvSet(operandA, temporaryValue);
 155             return temporaryValue;
 156             break;
 157
 158         case LOGIC_CONDITION_GVAR_DEC:
 159             temporaryValue = gvGet(operandA) - operandB;
 160             gvSet(operandA, temporaryValue);
 161             return temporaryValue;
 162             break;
 163
 164         case LOGIC_CONDITION_ADD:
 165             return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
 166             break;
 167
 168         case LOGIC_CONDITION_SUB:
 169             return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
 170             break;
 171
 172         case LOGIC_CONDITION_MUL:
 173             return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
 174             break;
 175
 176         case LOGIC_CONDITION_DIV:
 177             if (operandB != 0) {
 178                 return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
 179             } else {
 180                 return operandA;
 181             }
 182             break;
 183
 184         default:
 185             return false;
 186             break;
 187     }
 188 }
 189
 190 void logicConditionProcess(uint8_t i) {
 191
 192     const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
 193
 194     if (logicConditions(i)->enabled && activatorValue) {
 195
 196         /*
 197          * Process condition only when latch flag is not set
 198          * Latched LCs can only go from OFF to ON, not the other way
 199          */
 200         if (!(logicConditionStates[i].flags & LOGIC_CONDITION_FLAG_LATCH)) {
 201             const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
 202             const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
 203             const int newValue = logicConditionCompute(
 204                 logicConditionStates[i].value,
 205                 logicConditions(i)->operation,
 206                 operandAValue,
 207                 operandBValue
 208             );
 209
 210             logicConditionStates[i].value = newValue;
 211
 212             /*
 213              * if value evaluates as true, put a latch on logic condition
 214              */
 215             if (logicConditions(i)->flags & LOGIC_CONDITION_FLAG_LATCH && newValue) {
 216                 logicConditionStates[i].flags |= LOGIC_CONDITION_FLAG_LATCH;
 217             }
 218         }
 219     } else {
 220         logicConditionStates[i].value = false;
 221     }
 222 }
 223
 224 static int logicConditionGetFlightOperandValue(int operand) {
 225
 226     switch (operand) {
 227
 228         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
 229             return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
 230             break;
 231
 232         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
 233             return constrain(GPS_distanceToHome, 0, INT16_MAX);
 234             break;
 235
 236         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
 237             return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
 238             break;
 239
 240         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
 241             return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
 242             break;
 243
 244         case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10
 245             return getBatteryVoltage();
 246             break;
 247
 248         case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10
 249             return getBatteryAverageCellVoltage();
 250             break;
 251         case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
 252             return getAmperage();
 253             break;
 254
 255         case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
 256             return getMAhDrawn();
 257             break;
 258
 259         case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
 260             return gpsSol.numSat;
 261             break;
 262
 263         case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
 264             return gpsSol.groundSpeed;
 265             break;
 266
 267         //FIXME align with osdGet3DSpeed
 268         case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
 269             return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z)));
 270             break;
 271
 272         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
 273         #ifdef USE_PITOT
 274             return constrain(pitot.airSpeed, 0, INT16_MAX);
 275         #else
 276             return false;
 277         #endif
 278             break;
 279
 280         case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
 281             return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX);
 282             break;
 283
 284         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
 285             return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX);
 286             break;
 287
 288         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
 289             return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 290             break;
 291
 292         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg
 293             return constrain(attitude.values.roll / 10, -180, 180);
 294             break;
 295
 296         case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg
 297             return constrain(attitude.values.pitch / 10, -180, 180);
 298             break;
 299
 300         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1
 301             return ARMING_FLAG(ARMED) ? 1 : 0;
 302             break;
 303
 304         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
 305             return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
 306             break;
 307
 308         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
 309             return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
 310             break;
 311
 312         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
 313             return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
 314             break;
 315
 316         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
 317             return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
 318             break;
 319
 320         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
 321             return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
 322             break;
 323
 324         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1
 325             return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0;
 326             break;
 327
 328         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
 329             return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
 330             break;
 331
 332         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
 333             return (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) ? 1 : 0;
 334             break;
 335
 336         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
 337             return axisPID[YAW];
 338             break;
 339
 340         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
 341             return axisPID[ROLL];
 342             break;
 343
 344         case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
 345             return axisPID[PITCH];
 346             break;
 347
 348         default:
 349             return 0;
 350             break;
 351     }
 352 }
 353
 354 static int logicConditionGetFlightModeOperandValue(int operand) {
 355
 356     switch (operand) {
 357
 358         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE:
 359             return (bool) FLIGHT_MODE(FAILSAFE_MODE);
 360             break;
 361
 362         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL:
 363             return (bool) FLIGHT_MODE(MANUAL_MODE);
 364             break;
 365
 366         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH:
 367             return (bool) FLIGHT_MODE(NAV_RTH_MODE);
 368             break;
 369
 370         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD:
 371             return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
 372             break;
 373
 374         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
 375             return (bool) FLIGHT_MODE(NAV_CRUISE_MODE);
 376             break;
 377
 378         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
 379             return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE);
 380             break;
 381
 382         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE:
 383             return (bool) FLIGHT_MODE(ANGLE_MODE);
 384             break;
 385
 386         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON:
 387             return (bool) FLIGHT_MODE(HORIZON_MODE);
 388             break;
 389
 390         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
 391             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
 392             break;
 393
 394         default:
 395             return 0;
 396             break;
 397     }
 398 }
 399
 400 int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
 401     int retVal = 0;
 402
 403     switch (type) {
 404
 405         case LOGIC_CONDITION_OPERAND_TYPE_VALUE:
 406             retVal = operand;
 407             break;
 408
 409         case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL:
 410             //Extract RC channel raw value
 411             if (operand >= 1 && operand <= 16) {
 412                 retVal = rxGetChannelValue(operand - 1);
 413             }
 414             break;
 415
 416         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
 417             retVal = logicConditionGetFlightOperandValue(operand);
 418             break;
 419
 420         case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE:
 421             retVal = logicConditionGetFlightModeOperandValue(operand);
 422             break;
 423
 424         case LOGIC_CONDITION_OPERAND_TYPE_LC:
 425             if (operand >= 0 && operand < MAX_LOGIC_CONDITIONS) {
 426                 retVal = logicConditionGetValue(operand);
 427             }
 428             break;
 429
 430         case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
 431             if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
 432                 retVal = gvGet(operand);
 433             }
 434             break;
 435
 436         default:
 437             break;
 438     }
 439
 440     return retVal;
 441 }
 442
 443 /*
 444  * conditionId == -1 is always evaluated as true
 445  */
 446 int logicConditionGetValue(int8_t conditionId) {
 447     if (conditionId >= 0) {
 448         return logicConditionStates[conditionId].value;
 449     } else {
 450         return true;
 451     }
 452 }
 453
 454 void logicConditionUpdateTask(timeUs_t currentTimeUs) {
 455     UNUSED(currentTimeUs);
 456     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
 457         logicConditionProcess(i);
 458     }
 459 }
 460
 461 void logicConditionReset(void) {
 462     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
 463         logicConditionStates[i].value = 0;
 464         logicConditionStates[i].flags = 0;
 465     }
 466 }