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"
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_MODE_ACTIVATION_PROFILE, 1);
@ -75,48 +76,52 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
channelValue < 900 + (range->endStep * 25));
}
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, mode);
void updateMasksForMac(const modeActivationCondition_t *mac, boxBitmask_t *andMask, boxBitmask_t *newMask)
{
bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(andMask, mac->modeId);
bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range);
if (bAnd)
bitArraySet(andMask, mode);
bitArraySet(andMask, mac->modeId);
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)
{
boxBitmask_t newMask, andMask;
boxBitmask_t newMask, andMask, stickyModes;
memset(&andMask, 0, sizeof(andMask));
memset(&newMask, 0, sizeof(newMask));
memset(&stickyModes, 0, sizeof(stickyModes));
bitArraySet(&stickyModes, BOXPARALYZE);
// determine which conditions set/clear the mode
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i);
boxId_e mode = mac->modeId;
// Skip linked macs for now to fully determine target states
boxId_e linkedTo = mac->linkedTo;
if (linkedTo) {
if (mac->linkedTo) {
continue;
}
// Ensure sticky modes are sticky
if (mode == BOXPARALYZE) {
if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) {
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) {
if (bitArrayGet(&stickyModes, mac->modeId)) {
updateMasksForStickyModes(mac, &andMask, &newMask);
} else if (mac->modeId < CHECKBOX_ITEM_COUNT) {
updateMasksForMac(mac, &andMask, &newMask);
}
}
@ -127,13 +132,11 @@ void updateActivatedModes(void)
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i);
boxId_e linkedTo = mac->linkedTo;
if (!linkedTo) {
if (!mac->linkedTo) {
continue;
}
boxId_e mode = mac->modeId;
bitArrayCopy(&newMask, linkedTo, mode);
bitArrayCopy(&newMask, mac->linkedTo, mac->modeId);
}
rcModeUpdate(&newMask);