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