rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Generalize sticky mode code 

Instead of hard coding paralyze this makes the code extensible to possible
future sticky modes

_Legal disclaimer: I am making my contributions/submissions to this project solely in my personal capacity and am not conveying any rights to any intellectual property of any third parties._
This commit is contained in:
Robert Lacroix 2018-07-15 09:53:07 +02:00
parent cb792f30d2
commit a81576b2ae
1 changed files with 33 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/main/fc/rc_modes.c
View File

@ -40,9 +40,10 @@
#include "rx/rx.h" #include "rx/rx.h"
boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e #define STICKY_MODE_BOOT_DELAY_US 5e6
static bool paralyzeModeEverDisabled = false; boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
static boxBitmask_t stickyModesEverDisabled;
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions,
PG_MODE_ACTIVATION_PROFILE, 1); PG_MODE_ACTIVATION_PROFILE, 1);
@ -75,48 +76,52 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
channelValue < 900 + (range->endStep * 25)); channelValue < 900 + (range->endStep * 25));
} }
void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask) { void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
boxId_e mode = mac->modeId; {
bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(andMask, mac->modeId);
bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(andMask, mode);
bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range); bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range);
if (bAnd) if (bAnd)
bitArraySet(andMask, mode); bitArraySet(andMask, mac->modeId);
if (bAnd != bAct) if (bAnd != bAct)
bitArraySet(newMask, mode); bitArraySet(newMask, mac->modeId);
}
void updateMasksForStickyModes(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
{
if (IS_RC_MODE_ACTIVE(mac->modeId)) {
bitArrayClr(andMask, mac->modeId);
bitArraySet(newMask, mac->modeId);
} else {
if (bitArrayGet(&stickyModesEverDisabled, mac->modeId)) {
updateMasksForMac(mac, andMask, newMask);
} else {
if (micros() >= STICKY_MODE_BOOT_DELAY_US && !isRangeActive(mac->auxChannelIndex, &mac->range)) {
bitArraySet(&stickyModesEverDisabled, mac->modeId);
}
}
}
} }
void updateActivatedModes(void) void updateActivatedModes(void)
{ {
boxBitmask_t newMask, andMask; boxBitmask_t newMask, andMask, stickyModes;
memset(&andMask, 0, sizeof(andMask)); memset(&andMask, 0, sizeof(andMask));
memset(&newMask, 0, sizeof(newMask)); memset(&newMask, 0, sizeof(newMask));
memset(&stickyModes, 0, sizeof(stickyModes));
bitArraySet(&stickyModes, BOXPARALYZE);
// determine which conditions set/clear the mode // determine which conditions set/clear the mode
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i); const modeActivationCondition_t *mac = modeActivationConditions(i);
boxId_e mode = mac->modeId;
// Skip linked macs for now to fully determine target states // Skip linked macs for now to fully determine target states
boxId_e linkedTo = mac->linkedTo; if (mac->linkedTo) {
if (linkedTo) {
continue; continue;
} }
// Ensure sticky modes are sticky if (bitArrayGet(&stickyModes, mac->modeId)) {
if (mode == BOXPARALYZE) { updateMasksForStickyModes(mac, &andMask, &newMask);
if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { } else if (mac->modeId < CHECKBOX_ITEM_COUNT) {
bitArrayClr(&andMask, mode);
bitArraySet(&newMask, mode);
} else {
if (paralyzeModeEverDisabled) {
updateMasksForMac(mac, &andMask, &newMask);
} else {
paralyzeModeEverDisabled = micros() >= 5e6 && !isRangeActive(mac->auxChannelIndex, &mac->range);
}
}
} else if (mode < CHECKBOX_ITEM_COUNT) {
updateMasksForMac(mac, &andMask, &newMask); updateMasksForMac(mac, &andMask, &newMask);
} }
} }
@ -127,13 +132,11 @@ void updateActivatedModes(void)
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i); const modeActivationCondition_t *mac = modeActivationConditions(i);
boxId_e linkedTo = mac->linkedTo; if (!mac->linkedTo) {
if (!linkedTo) {
continue; continue;
} }
boxId_e mode = mac->modeId; bitArrayCopy(&newMask, mac->linkedTo, mac->modeId);
bitArrayCopy(&newMask, linkedTo, mode);
} }
rcModeUpdate(&newMask); rcModeUpdate(&newMask);