src/main/programming/logic_condition.h

   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 #pragma once
  25
  26 #include "config/parameter_group.h"
  27 #include "common/time.h"
  28
  29 #define MAX_LOGIC_CONDITIONS 64
  30
  31 typedef enum {
  32     LOGIC_CONDITION_TRUE                        = 0,
  33     LOGIC_CONDITION_EQUAL                       = 1,
  34     LOGIC_CONDITION_GREATER_THAN                = 2,
  35     LOGIC_CONDITION_LOWER_THAN                  = 3,
  36     LOGIC_CONDITION_LOW                         = 4,
  37     LOGIC_CONDITION_MID                         = 5,
  38     LOGIC_CONDITION_HIGH                        = 6,
  39     LOGIC_CONDITION_AND                         = 7,
  40     LOGIC_CONDITION_OR                          = 8,
  41     LOGIC_CONDITION_XOR                         = 9,
  42     LOGIC_CONDITION_NAND                        = 10,
  43     LOGIC_CONDITION_NOR                         = 11,
  44     LOGIC_CONDITION_NOT                         = 12,
  45     LOGIC_CONDITION_STICKY                      = 13,
  46     LOGIC_CONDITION_ADD                         = 14,
  47     LOGIC_CONDITION_SUB                         = 15,
  48     LOGIC_CONDITION_MUL                         = 16,
  49     LOGIC_CONDITION_DIV                         = 17,
  50     LOGIC_CONDITION_GVAR_SET                    = 18,
  51     LOGIC_CONDITION_GVAR_INC                    = 19,
  52     LOGIC_CONDITION_GVAR_DEC                    = 20,
  53     LOGIC_CONDITION_PORT_SET                    = 21,
  54     LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY      = 22,
  55     LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE     = 23,
  56     LOGIC_CONDITION_SWAP_ROLL_YAW               = 24,
  57     LOGIC_CONDITION_SET_VTX_POWER_LEVEL         = 25,
  58     LOGIC_CONDITION_INVERT_ROLL                 = 26,
  59     LOGIC_CONDITION_INVERT_PITCH                = 27,
  60     LOGIC_CONDITION_INVERT_YAW                  = 28,
  61     LOGIC_CONDITION_OVERRIDE_THROTTLE           = 29,
  62     LOGIC_CONDITION_SET_VTX_BAND                = 30,
  63     LOGIC_CONDITION_SET_VTX_CHANNEL             = 31,
  64     LOGIC_CONDITION_SET_OSD_LAYOUT              = 32,
  65     LOGIC_CONDITION_SIN                         = 33,
  66     LOGIC_CONDITION_COS                         = 34,
  67     LOGIC_CONDITION_TAN                         = 35,
  68     LOGIC_CONDITION_MAP_INPUT                   = 36,
  69     LOGIC_CONDITION_MAP_OUTPUT                  = 37,
  70     LOGIC_CONDITION_RC_CHANNEL_OVERRIDE         = 38,
  71     LOGIC_CONDITION_SET_HEADING_TARGET          = 39,
  72     LOGIC_CONDITION_MODULUS                     = 40,
  73     LOGIC_CONDITION_LOITER_OVERRIDE             = 41,
  74     LOGIC_CONDITION_SET_PROFILE                 = 42,
  75     LOGIC_CONDITION_MIN                         = 43,
  76     LOGIC_CONDITION_MAX                         = 44,
  77     LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE  = 45,
  78     LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE   = 46,
  79     LOGIC_CONDITION_EDGE                        = 47,
  80     LOGIC_CONDITION_DELAY                       = 48,
  81     LOGIC_CONDITION_TIMER                       = 49,
  82     LOGIC_CONDITION_DELTA                       = 50,
  83     LOGIC_CONDITION_APPROX_EQUAL                = 51,
  84     LOGIC_CONDITION_LED_PIN_PWM                 = 52,
  85     LOGIC_CONDITION_DISABLE_GPS_FIX             = 53,
  86     LOGIC_CONDITION_RESET_MAG_CALIBRATION       = 54,
  87     LOGIC_CONDITION_LAST                        = 55,
  88 } logicOperation_e;
  89
  90 typedef enum logicOperandType_s {
  91     LOGIC_CONDITION_OPERAND_TYPE_VALUE = 0,
  92     LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL,
  93     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT,
  94     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
  95     LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
  96     LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
  97     LOGIC_CONDITION_OPERAND_TYPE_PID,  // Value from a Programming PID
  98     LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS,
  99     LOGIC_CONDITION_OPERAND_TYPE_LAST
 100 } logicOperandType_e;
 101
 102 typedef enum {
 103     LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s                   // 0
 104     LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m                    // 1
 105     LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m                    // 2
 106     LOGIC_CONDITION_OPERAND_FLIGHT_RSSI,                                    // 3
 107     LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10                       // 4
 108     LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10               // 5
 109     LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100                    // 6
 110     LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh                        // 7
 111     LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS,                                // 8
 112     LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s                     // 9
 113     LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s                        // 10
 114     LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s                       // 11
 115     LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm                          // 12
 116     LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s                  // 13
 117     LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // %                        // 14
 118     LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg                    // 15
 119     LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg                   // 16
 120     LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED, // 0/1                         // 17
 121     LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH, // 0/1                    // 18
 122     LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL, // 0/1              // 19
 123     LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1              // 20
 124     LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1             // 21
 125     LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1                           // 22
 126     LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 23
 127     LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 24
 128     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 25
 129     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 26
 130     LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 27
 131     LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE,                        // 28
 132     LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 29
 133     LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 39
 134     LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 31
 135     LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 32
 136     LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 33
 137     LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS,                              // 34
 138     LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 35
 139     LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 36
 140     LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 37
 141     LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int              // 38
 142     LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1           // 39
 143     LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg                     // 40
 144     LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE,                           // 41
 145     LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int                     // 42
 146     LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS,                     // 43
 147 } logicFlightOperands_e;
 148
 149 typedef enum {
 150     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE,                           // 0
 151     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL,                             // 1
 152     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH,                                // 2
 153     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD,                            // 3
 154     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE,                             // 4
 155     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD,                            // 5
 156     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE,                              // 6
 157     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON,                            // 7
 158     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,                                // 8
 159     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1,                              // 9
 160     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2,                              // 10
 161     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD,                        // 11
 162     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3,                              // 12
 163     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4,                              // 13
 164     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO,                               // 14
 165     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION,                   // 15
 166     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD,                          // 16
 167 } logicFlightModeOperands_e;
 168
 169 typedef enum {
 170     LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1                         // 0
 171     LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX,                       // 1
 172     LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION,                      // 2
 173     LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION,                 // 3
 174     LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE,                    // 4
 175     LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT,               // 5
 176     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION,                         // 6
 177     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION,                         // 7
 178     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION,                         // 8
 179     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION,                         // 9
 180     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP,                 // 10
 181     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP,                 // 11
 182     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP,                 // 12
 183     LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP,                 // 13
 184 } logicWaypointOperands_e;
 185
 186 typedef enum {
 187     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
 188     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),
 189     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW = (1 << 2),
 190     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL = (1 << 3),
 191     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH = (1 << 4),
 192     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW = (1 << 5),
 193     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
 194     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
 195     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
 196     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
 197     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
 198 #ifdef USE_GPS_FIX_ESTIMATION
 199     LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
 200 #endif
 201 } logicConditionsGlobalFlags_t;
 202
 203 typedef enum {
 204     LOGIC_CONDITION_FLAG_LATCH              = 1 << 0,
 205     LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED  = 1 << 1,
 206 } logicConditionFlags_e;
 207
 208 typedef struct logicOperand_s {
 209     int32_t value;
 210     logicOperandType_e type;
 211 } logicOperand_t;
 212
 213 typedef struct logicCondition_s {
 214     logicOperand_t operandA;
 215     logicOperand_t operandB;
 216     uint8_t enabled;
 217     int8_t activatorId;
 218     logicOperation_e operation;
 219     uint8_t flags;
 220 } logicCondition_t;
 221
 222 PG_DECLARE_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions);
 223
 224 typedef struct logicConditionState_s {
 225     int value;
 226     int32_t lastValue;
 227     uint8_t flags;
 228     timeMs_t timeout;
 229 } logicConditionState_t;
 230
 231 typedef struct rcChannelOverride_s {
 232     int value;
 233     uint8_t active;
 234 } rcChannelOverride_t;
 235
 236 typedef struct flightAxisOverride_s {
 237     int angleTarget;
 238     int rateTarget;
 239     uint8_t rateTargetActive;
 240     uint8_t angleTargetActive;
 241 } flightAxisOverride_t;
 242
 243 extern int logicConditionValuesByType[LOGIC_CONDITION_LAST];
 244 extern uint64_t logicConditionsGlobalFlags;
 245
 246 #define LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(mask) (logicConditionsGlobalFlags &= ~(mask))
 247 #define LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(mask) (logicConditionsGlobalFlags |= (mask))
 248 #define LOGIC_CONDITION_GLOBAL_FLAG(mask) (logicConditionsGlobalFlags & (mask))
 249
 250 void logicConditionProcess(uint8_t i);
 251
 252 int32_t logicConditionGetOperandValue(logicOperandType_e type, int operand);
 253
 254 int32_t logicConditionGetValue(int8_t conditionId);
 255 void logicConditionUpdateTask(timeUs_t currentTimeUs);
 256 void logicConditionReset(void);
 257
 258 float getThrottleScale(float globalThrottleScale);
 259 int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
 260 int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
 261 uint32_t getLoiterRadius(uint32_t loiterRadius);
 262 float getFlightAxisAngleOverride(uint8_t axis, float angle);
 263 float getFlightAxisRateOverride(uint8_t axis, float rate);
 264 bool isFlightAxisAngleOverrideActive(uint8_t axis);
 265 bool isFlightAxisRateOverrideActive(uint8_t axis);