src/main/fc/rc_modes.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "rc_modes.h"
  25
  26 #include "common/bitarray.h"
  27 #include "common/maths.h"
  28 #include "common/utils.h"
  29
  30 #include "config/feature.h"
  31 #include "config/parameter_group.h"
  32 #include "config/parameter_group_ids.h"
  33
  34 #include "fc/config.h"
  35 #include "fc/rc_controls.h"
  36 #include "fc/runtime_config.h"
  37 #include "fc/settings.h"
  38
  39 #include "rx/rx.h"
  40
  41 static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
  42 static bool isUsingNAVModes = false;
  43
  44 boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
  45
  46 // TODO(alberto): It looks like we can now safely remove this assert, since everything
  47 // but BB is able to handle more than 32 boxes and all the definitions use
  48 // CHECKBOX_ITEM_COUNT rather than hardcoded values. Note, however, that BB will only
  49 // log the first 32 flight modes, so the ones affecting actual flight should be <= 32.
  50 //
  51 // Leaving the assert commented for now, just in case there are some unexpected issues
  52 // and someone else has to debug it.
  53 // STATIC_ASSERT(CHECKBOX_ITEM_COUNT <= 32, too_many_box_modes);
  54
  55 PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
  56 PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
  57
  58 PG_RESET_TEMPLATE(modeActivationOperatorConfig_t, modeActivationOperatorConfig,
  59     .modeActivationOperator = SETTING_MODE_RANGE_LOGIC_OPERATOR_DEFAULT
  60 );
  61
  62 static void processAirmodeAirplane(void) {
  63     if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
  64         ENABLE_STATE(AIRMODE_ACTIVE);
  65     } else {
  66         DISABLE_STATE(AIRMODE_ACTIVE);
  67     }
  68 }
  69
  70 static void processAirmodeMultirotor(void) {
  71     if ((rcControlsConfig()->airmodeHandlingType == STICK_CENTER) || (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE)) {
  72         if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
  73             ENABLE_STATE(AIRMODE_ACTIVE);
  74         } else {
  75             DISABLE_STATE(AIRMODE_ACTIVE);
  76         }
  77     } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
  78
  79         if (!ARMING_FLAG(ARMED)) {
  80             /*
  81              * Disarm disables airmode immediately
  82              */
  83             DISABLE_STATE(AIRMODE_ACTIVE);
  84         } else if (
  85             !STATE(AIRMODE_ACTIVE) &&
  86             rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold &&
  87             (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE))
  88         ) {
  89             /*
  90              * Airmode is allowed to be active only after ARMED and then THROTTLE goes above
  91              * activation threshold
  92              */
  93             ENABLE_STATE(AIRMODE_ACTIVE);
  94         } else if (
  95             STATE(AIRMODE_ACTIVE) &&
  96             !feature(FEATURE_AIRMODE) &&
  97             !IS_RC_MODE_ACTIVE(BOXAIRMODE)
  98         ) {
  99             /*
 100              *  When user disables BOXAIRMODE, turn airmode off as well
 101              */
 102             DISABLE_STATE(AIRMODE_ACTIVE);
 103         }
 104
 105     } else {
 106         DISABLE_STATE(AIRMODE_ACTIVE);
 107     }
 108 }
 109
 110 void processAirmode(void) {
 111
 112     if (STATE(AIRPLANE)) {
 113         processAirmodeAirplane();
 114     } else if (STATE(MULTIROTOR)) {
 115         processAirmodeMultirotor();
 116     }
 117 }
 118
 119 bool isUsingNavigationModes(void)
 120 {
 121     return isUsingNAVModes;
 122 }
 123
 124 bool isFwAutoModeActive(boxId_e mode)
 125 {
 126     /* Sets activation priority of fixed wing auto tune/trim modes: Autotune -> Autotrim -> Autolevel */
 127
 128     if (mode == BOXAUTOTUNE) {
 129         return IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
 130     } else if (mode == BOXAUTOTRIM) {
 131         return IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
 132     } else if (mode == BOXAUTOLEVEL) {
 133         return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && !IS_RC_MODE_ACTIVE(BOXAUTOTRIM);
 134     }
 135
 136     return false;
 137 }
 138
 139 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
 140 {
 141     return bitArrayGet(rcModeActivationMask.bits, boxId);
 142 }
 143
 144 void rcModeUpdate(boxBitmask_t *newState)
 145 {
 146     rcModeActivationMask = *newState;
 147 }
 148
 149 bool isModeActivationConditionPresent(boxId_e modeId)
 150 {
 151     return specifiedConditionCountPerMode[modeId] > 0;
 152 }
 153
 154 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
 155 {
 156     if (!IS_RANGE_USABLE(range)) {
 157         return false;
 158     }
 159
 160     // No need to constrain() here, since we're testing for a closed range defined
 161     // by the channelRange_t. If channelValue has an invalid value, the test will
 162     // be false anyway.
 163     uint16_t channelValue = rxGetChannelValue(auxChannelIndex + NON_AUX_CHANNEL_COUNT);
 164     return (channelValue >= CHANNEL_RANGE_MIN + (range->startStep * CHANNEL_RANGE_STEP_WIDTH) &&
 165             channelValue < CHANNEL_RANGE_MIN + (range->endStep * CHANNEL_RANGE_STEP_WIDTH));
 166 }
 167
 168 void updateActivatedModes(void)
 169 {
 170     // Disable all modes to begin with
 171     boxBitmask_t newMask;
 172     memset(&newMask, 0, sizeof(newMask));
 173
 174     // Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
 175     // We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
 176     uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
 177     memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
 178
 179     for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 180         if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
 181             // Increment the number of valid conditions for this mode
 182             activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
 183         }
 184     }
 185
 186     // Now see which modes should be enabled
 187     for (int modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) {
 188         // only modes with conditions specified are considered
 189         if (specifiedConditionCountPerMode[modeIndex] > 0) {
 190             // For AND logic, the specified condition count and valid condition count must be the same.
 191             // For OR logic, the valid condition count must be greater than zero.
 192
 193             if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
 194                 // AND the conditions
 195                 if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
 196                     bitArraySet(newMask.bits, modeIndex);
 197                 }
 198             }
 199             else {
 200                 // OR the conditions
 201                 if (activeConditionCountPerMode[modeIndex] > 0) {
 202                     bitArraySet(newMask.bits, modeIndex);
 203                 }
 204             }
 205         }
 206     }
 207
 208     rcModeUpdate(&newMask);
 209 }
 210
 211 void updateUsedModeActivationConditionFlags(void)
 212 {
 213     memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
 214     for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 215         if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
 216             specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
 217         }
 218     }
 219
 220     isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
 221                         isModeActivationConditionPresent(BOXNAVRTH) ||
 222                         isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
 223                         isModeActivationConditionPresent(BOXNAVCRUISE) ||
 224                         isModeActivationConditionPresent(BOXNAVWP);
 225 }