src/main/fc/rc_modes.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #include "rc_modes.h"
  28
  29 #include "common/bitarray.h"
  30 #include "common/maths.h"
  31 #include "drivers/time.h"
  32
  33 #include "config/feature.h"
  34 #include "pg/pg.h"
  35 #include "pg/pg_ids.h"
  36 #include "pg/rx.h"
  37
  38 #include "config/config.h"
  39 #include "fc/rc_controls.h"
  40
  41 #include "rx/rx.h"
  42
  43 #define STICKY_MODE_BOOT_DELAY_US 5e6
  44
  45 boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
  46 static boxBitmask_t stickyModesEverDisabled;
  47
  48 static bool airmodeEnabled;
  49
  50 static int activeMacCount = 0;
  51 static uint8_t activeMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
  52 static int activeLinkedMacCount = 0;
  53 static uint8_t activeLinkedMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
  54
  55 PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 2);
  56
  57 #if defined(USE_CUSTOM_BOX_NAMES)
  58 PG_REGISTER_WITH_RESET_TEMPLATE(modeActivationConfig_t, modeActivationConfig, PG_MODE_ACTIVATION_CONFIG, 0);
  59
  60 PG_RESET_TEMPLATE(modeActivationConfig_t, modeActivationConfig,
  61     .box_user_1_name = { 0 },
  62     .box_user_2_name = { 0 },
  63     .box_user_3_name = { 0 },
  64     .box_user_4_name = { 0 },
  65 );
  66 #endif
  67
  68 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
  69 {
  70     return bitArrayGet(&rcModeActivationMask, boxId);
  71 }
  72
  73 void rcModeUpdate(boxBitmask_t *newState)
  74 {
  75     rcModeActivationMask = *newState;
  76 }
  77
  78 bool airmodeIsEnabled(void) {
  79     return airmodeEnabled;
  80 }
  81
  82 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
  83     if (!IS_RANGE_USABLE(range)) {
  84         return false;
  85     }
  86
  87     const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
  88     return (channelValue >= 900 + (range->startStep * 25) &&
  89             channelValue < 900 + (range->endStep * 25));
  90 }
  91
  92 /*
  93  *  updateMasksForMac:
  94  *
  95  *  The following are the possible logic states at each MAC update:
  96  *      AND     NEW
  97  *      ---     ---
  98  *       F       F      - no previous AND macs evaluated, no previous active OR macs
  99  *       F       T      - at least 1 previous active OR mac (***this state is latched True***)
 100  *       T       F      - all previous AND macs active, no previous active OR macs
 101  *       T       T      - at least 1 previous inactive AND mac, no previous active OR macs
 102  */
 103 void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask, bool bActive)
 104 {
 105     if (bitArrayGet(andMask, mac->modeId) || !bitArrayGet(newMask, mac->modeId)) {
 106         bool bAnd = mac->modeLogic == MODELOGIC_AND;
 107
 108         if (!bAnd) {    // OR mac
 109             if (bActive) {
 110                 bitArrayClr(andMask, mac->modeId);
 111                 bitArraySet(newMask, mac->modeId);
 112             }
 113         } else {        // AND mac
 114             bitArraySet(andMask, mac->modeId);
 115             if (!bActive) {
 116                 bitArraySet(newMask, mac->modeId);
 117             }
 118         }
 119     }
 120 }
 121
 122 void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
 123 {
 124     if (IS_RC_MODE_ACTIVE(mac->modeId)) {
 125         bitArrayClr(andMask, mac->modeId);
 126         bitArraySet(newMask, mac->modeId);
 127     } else {
 128         bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 129
 130         if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) {
 131             updateMasksForMac(mac, andMask, newMask, bActive);
 132         } else {
 133             if (micros() >= STICKY_MODE_BOOT_DELAY_US && !bActive) {
 134                 bitArraySet(&stickyModesEverDisabled, mac->modeId);
 135             }
 136         }
 137     }
 138 }
 139
 140 void updateActivatedModes(void)
 141 {
 142     boxBitmask_t newMask, andMask, stickyModes;
 143     memset(&andMask, 0, sizeof(andMask));
 144     memset(&newMask, 0, sizeof(newMask));
 145     memset(&stickyModes, 0, sizeof(stickyModes));
 146     bitArraySet(&stickyModes, BOXPARALYZE);
 147
 148     // determine which conditions set/clear the mode
 149     for (int i = 0; i < activeMacCount; i++) {
 150         const modeActivationCondition_t *mac = modeActivationConditions(activeMacArray[i]);
 151
 152         if (bitArrayGet(&stickyModes, mac->modeId)) {
 153             updateMasksForStickyModes(mac, &andMask, &newMask);
 154         } else if (mac->modeId < CHECKBOX_ITEM_COUNT) {
 155             bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 156             updateMasksForMac(mac, &andMask, &newMask, bActive);
 157         }
 158     }
 159
 160     // Update linked modes
 161     for (int i = 0; i < activeLinkedMacCount; i++) {
 162         const modeActivationCondition_t *mac = modeActivationConditions(activeLinkedMacArray[i]);
 163         bool bActive = bitArrayGet(&andMask, mac->linkedTo) != bitArrayGet(&newMask, mac->linkedTo);
 164
 165         updateMasksForMac(mac, &andMask, &newMask, bActive);
 166     }
 167
 168     bitArrayXor(&newMask, sizeof(newMask), &newMask, &andMask);
 169
 170     rcModeUpdate(&newMask);
 171
 172     airmodeEnabled = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
 173 }
 174
 175 bool isModeActivationConditionPresent(boxId_e modeId)
 176 {
 177     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 178         const modeActivationCondition_t *mac = modeActivationConditions(i);
 179
 180         if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) {
 181             return true;
 182         }
 183     }
 184
 185     return false;
 186 }
 187
 188 bool isModeActivationConditionLinked(boxId_e modeId)
 189 {
 190     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 191         const modeActivationCondition_t *mac = modeActivationConditions(i);
 192
 193         if (mac->modeId == modeId && mac->linkedTo != 0) {
 194             return true;
 195         }
 196     }
 197
 198     return false;
 199 }
 200
 201 void removeModeActivationCondition(const boxId_e modeId)
 202 {
 203     unsigned offset = 0;
 204     for (unsigned i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 205         modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
 206
 207         if (mac->modeId == modeId && !offset) {
 208             offset++;
 209         }
 210
 211         if (offset) {
 212             while (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT && modeActivationConditions(i + offset)->modeId == modeId) {
 213                 offset++;
 214             }
 215
 216             if (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
 217                 memcpy(mac, modeActivationConditions(i + offset), sizeof(modeActivationCondition_t));
 218             } else {
 219                 memset(mac, 0, sizeof(modeActivationCondition_t));
 220             }
 221         }
 222     }
 223 }
 224
 225 bool isModeActivationConditionConfigured(const modeActivationCondition_t *mac, const modeActivationCondition_t *emptyMac)
 226 {
 227     if (memcmp(mac, emptyMac, sizeof(*emptyMac))) {
 228         return true;
 229     } else {
 230         return false;
 231     }
 232 }
 233
 234 // Build the list of used modeActivationConditions indices
 235 // We can then use this to speed up processing by only evaluating used conditions
 236 void analyzeModeActivationConditions(void)
 237 {
 238     modeActivationCondition_t emptyMac;
 239     memset(&emptyMac, 0, sizeof(emptyMac));
 240
 241     activeMacCount = 0;
 242     activeLinkedMacCount = 0;
 243
 244     for (uint8_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 245         const modeActivationCondition_t *mac = modeActivationConditions(i);
 246         if (mac->linkedTo) {
 247             activeLinkedMacArray[activeLinkedMacCount++] = i;
 248         } else if (isModeActivationConditionConfigured(mac, &emptyMac)) {
 249             activeMacArray[activeMacCount++] = i;
 250         }
 251     }
 252 }