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 "common/bitarray.h"
  28 #include "common/maths.h"
  29
  30 #include "drivers/time.h"
  31
  32 #include "config/config.h"
  33 #include "config/feature.h"
  34
  35 #include "fc/rc_controls.h"
  36
  37 #include "io/piniobox.h"
  38
  39 #include "pg/pg.h"
  40 #include "pg/pg_ids.h"
  41 #include "pg/rx.h"
  42
  43 #include "rx/rx.h"
  44
  45 #include "rc_modes.h"
  46
  47 #define STICKY_MODE_BOOT_DELAY_US 5e6
  48
  49 boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
  50 static boxBitmask_t stickyModesEverDisabled;
  51
  52 static bool airmodeEnabled;
  53
  54 static int activeMacCount = 0;
  55 static uint8_t activeMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
  56 static int activeLinkedMacCount = 0;
  57 static uint8_t activeLinkedMacArray[MAX_MODE_ACTIVATION_CONDITION_COUNT];
  58
  59 PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 2);
  60
  61 #if defined(USE_CUSTOM_BOX_NAMES)
  62 PG_REGISTER_WITH_RESET_TEMPLATE(modeActivationConfig_t, modeActivationConfig, PG_MODE_ACTIVATION_CONFIG, 0);
  63
  64 PG_RESET_TEMPLATE(modeActivationConfig_t, modeActivationConfig,
  65     .box_user_1_name = { 0 },
  66     .box_user_2_name = { 0 },
  67     .box_user_3_name = { 0 },
  68     .box_user_4_name = { 0 },
  69 );
  70 #endif
  71
  72 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
  73 {
  74     return bitArrayGet(&rcModeActivationMask, boxId);
  75 }
  76
  77 void rcModeUpdate(boxBitmask_t *newState)
  78 {
  79     rcModeActivationMask = *newState;
  80 }
  81
  82 bool airmodeIsEnabled(void)
  83 {
  84     return airmodeEnabled;
  85 }
  86
  87 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
  88 {
  89     if (!IS_RANGE_USABLE(range)) {
  90         return false;
  91     }
  92
  93     const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
  94     return (channelValue >= 900 + (range->startStep * 25) &&
  95             channelValue < 900 + (range->endStep * 25));
  96 }
  97
  98 /*
  99  *  updateMasksForMac:
 100  *
 101  *  The following are the possible logic states at each MAC update:
 102  *      AND     NEW
 103  *      ---     ---
 104  *       F       F      - no previous AND macs evaluated, no previous active OR macs
 105  *       F       T      - at least 1 previous active OR mac (***this state is latched True***)
 106  *       T       F      - all previous AND macs active, no previous active OR macs
 107  *       T       T      - at least 1 previous inactive AND mac, no previous active OR macs
 108  */
 109 void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask, bool bActive)
 110 {
 111     if (bitArrayGet(andMask, mac->modeId) || !bitArrayGet(newMask, mac->modeId)) {
 112         bool bAnd = mac->modeLogic == MODELOGIC_AND;
 113
 114         if (!bAnd) {    // OR mac
 115             if (bActive) {
 116                 bitArrayClr(andMask, mac->modeId);
 117                 bitArraySet(newMask, mac->modeId);
 118             }
 119         } else {        // AND mac
 120             bitArraySet(andMask, mac->modeId);
 121             if (!bActive) {
 122                 bitArraySet(newMask, mac->modeId);
 123             }
 124         }
 125     }
 126 }
 127
 128 void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
 129 {
 130     if (IS_RC_MODE_ACTIVE(mac->modeId)) {
 131         bitArrayClr(andMask, mac->modeId);
 132         bitArraySet(newMask, mac->modeId);
 133     } else {
 134         bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 135
 136         if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) {
 137             updateMasksForMac(mac, andMask, newMask, bActive);
 138         } else {
 139             if (micros() >= STICKY_MODE_BOOT_DELAY_US && !bActive) {
 140                 bitArraySet(&stickyModesEverDisabled, mac->modeId);
 141             }
 142         }
 143     }
 144 }
 145
 146 void updateActivatedModes(void)
 147 {
 148     boxBitmask_t newMask, andMask, stickyModes;
 149     memset(&andMask, 0, sizeof(andMask));
 150     memset(&newMask, 0, sizeof(newMask));
 151     memset(&stickyModes, 0, sizeof(stickyModes));
 152     bitArraySet(&stickyModes, BOXPARALYZE);
 153
 154     // determine which conditions set/clear the mode
 155     for (int i = 0; i < activeMacCount; i++) {
 156         const modeActivationCondition_t *mac = modeActivationConditions(activeMacArray[i]);
 157
 158         if (bitArrayGet(&stickyModes, mac->modeId)) {
 159             updateMasksForStickyModes(mac, &andMask, &newMask);
 160         } else if (mac->modeId < CHECKBOX_ITEM_COUNT) {
 161             bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 162             updateMasksForMac(mac, &andMask, &newMask, bActive);
 163         }
 164     }
 165
 166     // Update linked modes
 167     for (int i = 0; i < activeLinkedMacCount; i++) {
 168         const modeActivationCondition_t *mac = modeActivationConditions(activeLinkedMacArray[i]);
 169         bool bActive = bitArrayGet(&andMask, mac->linkedTo) != bitArrayGet(&newMask, mac->linkedTo);
 170
 171         updateMasksForMac(mac, &andMask, &newMask, bActive);
 172     }
 173
 174     bitArrayXor(&newMask, sizeof(newMask), &newMask, &andMask);
 175
 176     rcModeUpdate(&newMask);
 177
 178     airmodeEnabled = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
 179 }
 180
 181 bool isModeActivationConditionPresent(boxId_e modeId)
 182 {
 183     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 184         const modeActivationCondition_t *mac = modeActivationConditions(i);
 185
 186         if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) {
 187             return true;
 188         }
 189     }
 190
 191     return false;
 192 }
 193
 194 bool isModeActivationConditionLinked(boxId_e modeId)
 195 {
 196     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 197         const modeActivationCondition_t *mac = modeActivationConditions(i);
 198
 199         if (mac->modeId == modeId && mac->linkedTo != 0) {
 200             return true;
 201         }
 202     }
 203
 204     return false;
 205 }
 206
 207 void removeModeActivationCondition(const boxId_e modeId)
 208 {
 209     unsigned offset = 0;
 210     for (unsigned i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 211         modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
 212
 213         if (mac->modeId == modeId && !offset) {
 214             offset++;
 215         }
 216
 217         if (offset) {
 218             while (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT && modeActivationConditions(i + offset)->modeId == modeId) {
 219                 offset++;
 220             }
 221
 222             if (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
 223                 memcpy(mac, modeActivationConditions(i + offset), sizeof(modeActivationCondition_t));
 224             } else {
 225                 memset(mac, 0, sizeof(modeActivationCondition_t));
 226             }
 227         }
 228     }
 229 }
 230
 231 bool isModeActivationConditionConfigured(const modeActivationCondition_t *mac, const modeActivationCondition_t *emptyMac)
 232 {
 233     if (memcmp(mac, emptyMac, sizeof(*emptyMac))) {
 234         return true;
 235     } else {
 236         return false;
 237     }
 238 }
 239
 240 // Build the list of used modeActivationConditions indices
 241 // We can then use this to speed up processing by only evaluating used conditions
 242 void analyzeModeActivationConditions(void)
 243 {
 244     modeActivationCondition_t emptyMac;
 245     memset(&emptyMac, 0, sizeof(emptyMac));
 246
 247     activeMacCount = 0;
 248     activeLinkedMacCount = 0;
 249
 250     for (uint8_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 251         const modeActivationCondition_t *mac = modeActivationConditions(i);
 252         if (mac->linkedTo) {
 253             activeLinkedMacArray[activeLinkedMacCount++] = i;
 254         } else if (isModeActivationConditionConfigured(mac, &emptyMac)) {
 255             activeMacArray[activeMacCount++] = i;
 256         }
 257     }
 258 #ifdef USE_PINIOBOX
 259     pinioBoxTaskControl();
 260 #endif
 261 }