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     return airmodeEnabled;
  84 }
  85
  86 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
  87     if (!IS_RANGE_USABLE(range)) {
  88         return false;
  89     }
  90
  91     const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1);
  92     return (channelValue >= 900 + (range->startStep * 25) &&
  93             channelValue < 900 + (range->endStep * 25));
  94 }
  95
  96 /*
  97  *  updateMasksForMac:
  98  *
  99  *  The following are the possible logic states at each MAC update:
 100  *      AND     NEW
 101  *      ---     ---
 102  *       F       F      - no previous AND macs evaluated, no previous active OR macs
 103  *       F       T      - at least 1 previous active OR mac (***this state is latched True***)
 104  *       T       F      - all previous AND macs active, no previous active OR macs
 105  *       T       T      - at least 1 previous inactive AND mac, no previous active OR macs
 106  */
 107 void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask, bool bActive)
 108 {
 109     if (bitArrayGet(andMask, mac->modeId) || !bitArrayGet(newMask, mac->modeId)) {
 110         bool bAnd = mac->modeLogic == MODELOGIC_AND;
 111
 112         if (!bAnd) {    // OR mac
 113             if (bActive) {
 114                 bitArrayClr(andMask, mac->modeId);
 115                 bitArraySet(newMask, mac->modeId);
 116             }
 117         } else {        // AND mac
 118             bitArraySet(andMask, mac->modeId);
 119             if (!bActive) {
 120                 bitArraySet(newMask, mac->modeId);
 121             }
 122         }
 123     }
 124 }
 125
 126 void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
 127 {
 128     if (IS_RC_MODE_ACTIVE(mac->modeId)) {
 129         bitArrayClr(andMask, mac->modeId);
 130         bitArraySet(newMask, mac->modeId);
 131     } else {
 132         bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 133
 134         if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) {
 135             updateMasksForMac(mac, andMask, newMask, bActive);
 136         } else {
 137             if (micros() >= STICKY_MODE_BOOT_DELAY_US && !bActive) {
 138                 bitArraySet(&stickyModesEverDisabled, mac->modeId);
 139             }
 140         }
 141     }
 142 }
 143
 144 void updateActivatedModes(void)
 145 {
 146     boxBitmask_t newMask, andMask, stickyModes;
 147     memset(&andMask, 0, sizeof(andMask));
 148     memset(&newMask, 0, sizeof(newMask));
 149     memset(&stickyModes, 0, sizeof(stickyModes));
 150     bitArraySet(&stickyModes, BOXPARALYZE);
 151
 152     // determine which conditions set/clear the mode
 153     for (int i = 0; i < activeMacCount; i++) {
 154         const modeActivationCondition_t *mac = modeActivationConditions(activeMacArray[i]);
 155
 156         if (bitArrayGet(&stickyModes, mac->modeId)) {
 157             updateMasksForStickyModes(mac, &andMask, &newMask);
 158         } else if (mac->modeId < CHECKBOX_ITEM_COUNT) {
 159             bool bActive = isRangeActive(mac->auxChannelIndex, &mac->range);
 160             updateMasksForMac(mac, &andMask, &newMask, bActive);
 161         }
 162     }
 163
 164     // Update linked modes
 165     for (int i = 0; i < activeLinkedMacCount; i++) {
 166         const modeActivationCondition_t *mac = modeActivationConditions(activeLinkedMacArray[i]);
 167         bool bActive = bitArrayGet(&andMask, mac->linkedTo) != bitArrayGet(&newMask, mac->linkedTo);
 168
 169         updateMasksForMac(mac, &andMask, &newMask, bActive);
 170     }
 171
 172     bitArrayXor(&newMask, sizeof(newMask), &newMask, &andMask);
 173
 174     rcModeUpdate(&newMask);
 175
 176     airmodeEnabled = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
 177 }
 178
 179 bool isModeActivationConditionPresent(boxId_e modeId)
 180 {
 181     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 182         const modeActivationCondition_t *mac = modeActivationConditions(i);
 183
 184         if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) {
 185             return true;
 186         }
 187     }
 188
 189     return false;
 190 }
 191
 192 bool isModeActivationConditionLinked(boxId_e modeId)
 193 {
 194     for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 195         const modeActivationCondition_t *mac = modeActivationConditions(i);
 196
 197         if (mac->modeId == modeId && mac->linkedTo != 0) {
 198             return true;
 199         }
 200     }
 201
 202     return false;
 203 }
 204
 205 void removeModeActivationCondition(const boxId_e modeId)
 206 {
 207     unsigned offset = 0;
 208     for (unsigned i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 209         modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
 210
 211         if (mac->modeId == modeId && !offset) {
 212             offset++;
 213         }
 214
 215         if (offset) {
 216             while (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT && modeActivationConditions(i + offset)->modeId == modeId) {
 217                 offset++;
 218             }
 219
 220             if (i + offset < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
 221                 memcpy(mac, modeActivationConditions(i + offset), sizeof(modeActivationCondition_t));
 222             } else {
 223                 memset(mac, 0, sizeof(modeActivationCondition_t));
 224             }
 225         }
 226     }
 227 }
 228
 229 bool isModeActivationConditionConfigured(const modeActivationCondition_t *mac, const modeActivationCondition_t *emptyMac)
 230 {
 231     if (memcmp(mac, emptyMac, sizeof(*emptyMac))) {
 232         return true;
 233     } else {
 234         return false;
 235     }
 236 }
 237
 238 // Build the list of used modeActivationConditions indices
 239 // We can then use this to speed up processing by only evaluating used conditions
 240 void analyzeModeActivationConditions(void)
 241 {
 242     modeActivationCondition_t emptyMac;
 243     memset(&emptyMac, 0, sizeof(emptyMac));
 244
 245     activeMacCount = 0;
 246     activeLinkedMacCount = 0;
 247
 248     for (uint8_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
 249         const modeActivationCondition_t *mac = modeActivationConditions(i);
 250         if (mac->linkedTo) {
 251             activeLinkedMacArray[activeLinkedMacCount++] = i;
 252         } else if (isModeActivationConditionConfigured(mac, &emptyMac)) {
 253             activeMacArray[activeMacCount++] = i;
 254         }
 255     }
 256 #ifdef USE_PINIOBOX
 257     pinioBoxTaskControl();
 258 #endif
 259 }