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

Allow linking of modes 

This allows modes to be linked, for example to link vtx pit mode to paralyze. Whenever paralyze is activated, vtx pit mode is activated as well. Also the logic to prevent mode changes when enabling paralyze can be removed in favor of making paralyze sticky.

Modes can be linked in CLI by providing the mode id as the last parameter of the aux command. For example in order to link vtx pit mode to paralyze, replace the last 0 of the pit mode aux (39) with the mode id of paralyze (45):

```
aux 2 39 2 1700 2100 0 0
```

becomes

```
aux 2 39 2 1700 2100 0 45
```

_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-11 12:04:11 +02:00
parent ac2f39a10c
commit aa18ab4afa
7 changed files with 130 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/common/bitarray.c
View File

@ -47,3 +47,12 @@ void bitArrayXor(void *dest, size_t size, void *op1, void *op2)
((uint8_t*)dest)[i] = ((uint8_t*)op1)[i] ^ ((uint8_t*)op2)[i];
}
}
void bitArrayCopy(void *array, unsigned from, unsigned to)
{
if (bitArrayGet(array, from)) {
bitArraySet(array, to);
} else {
bitArrayClr(array, to);
}
}

1
src/main/common/bitarray.h
View File

@ -24,3 +24,4 @@ bool bitArrayGet(const void *array, unsigned bit);
void bitArraySet(void *array, unsigned bit);
void bitArrayClr(void *array, unsigned bit);
void bitArrayXor(void *dest, size_t size, void *op1, void *op2);
void bitArrayCopy(void *array, unsigned from, unsigned to);

20
src/main/fc/fc_core.c
View File

@ -55,7 +55,6 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_core.h"
#include "fc/fc_dispatch.h"
#include "fc/fc_rc.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
@ -120,8 +119,6 @@ enum {
#define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
#endif
#define PARALYZE_PREVENT_MODE_CHANGES_DELAY_US 100000 // Delay after paralyze mode activates to let other mode changes propagate
#if defined(USE_GPS) || defined(USE_MAG)
int16_t magHold;
#endif
@ -143,15 +140,6 @@ static timeUs_t runawayTakeoffTriggerUs = 0;
static bool runawayTakeoffTemporarilyDisabled = false;
#endif
static bool paralyzeModeAllowed = false;
void preventModeChangesDispatch(dispatchEntry_t *self) {
UNUSED(self);
preventModeChanges();
}
static dispatchEntry_t preventModeChangesDispatchEntry = { .dispatch = preventModeChangesDispatch};
PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
@ -265,9 +253,8 @@ void updateArmingStatus(void)
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPARALYZE) && paralyzeModeAllowed) {
if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) {
setArmingDisabled(ARMING_DISABLED_PARALYZE);
dispatchAdd(&preventModeChangesDispatchEntry, PARALYZE_PREVENT_MODE_CHANGES_DELAY_US);
}
if (!isUsingSticksForArming()) {
@ -306,11 +293,6 @@ void updateArmingStatus(void)
warningLedUpdate();
}
// Check if entering into paralyze mode can be allowed regardless of arming status
if (!IS_RC_MODE_ACTIVE(BOXPARALYZE) && !paralyzeModeAllowed) {
paralyzeModeAllowed = true;
}
}
void disarm(void)

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

@ -40,7 +40,8 @@
#include "rx/rx.h"
boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
static bool modeChangesDisabled = false;
static bool paralyzeModeEverDisabled = false;
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions,
PG_MODE_ACTIVATION_PROFILE, 1);
@ -55,10 +56,6 @@ void rcModeUpdate(boxBitmask_t *newState)
rcModeActivationMask = *newState;
}
void preventModeChanges(void) {
modeChangesDisabled = true;
}
bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
@ -77,17 +74,22 @@ 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);
bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range);
if (bAnd)
bitArraySet(andMask, mode);
if (bAnd != bAct)
bitArraySet(newMask, mode);
}
void updateActivatedModes(void)
{
boxBitmask_t newMask, andMask;
memset(&andMask, 0, sizeof(andMask));
if (!modeChangesDisabled) {
memset(&newMask, 0, sizeof(newMask));
} else {
memcpy(&newMask, &rcModeActivationMask, sizeof(newMask));
}
memset(&newMask, 0, sizeof(newMask));
// determine which conditions set/clear the mode
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@ -95,21 +97,44 @@ void updateActivatedModes(void)
boxId_e mode = mac->modeId;
if (modeChangesDisabled && mode != BOXBEEPERON) {
// Skip linked macs for now to fully determine target states
boxId_e linkedTo = mac->linkedTo;
if (linkedTo) {
continue;
}
if (mode < CHECKBOX_ITEM_COUNT) {
bool bAnd = (mac->modeLogic == MODELOGIC_AND) || bitArrayGet(&andMask, mode);
bool bAct = isRangeActive(mac->auxChannelIndex, &mac->range);
if (bAnd)
bitArraySet(&andMask, mode);
if (bAnd != bAct)
// 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 = !isRangeActive(mac->auxChannelIndex, &mac->range);
}
}
} else if (mode < CHECKBOX_ITEM_COUNT) {
updateMasksForMac(mac, &andMask, &newMask);
}
}
bitArrayXor(&newMask, sizeof(&newMask), &newMask, &andMask);
// Update linked modes
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i);
boxId_e linkedTo = mac->linkedTo;
if (!linkedTo) {
continue;
}
boxId_e mode = mac->modeId;
bitArrayCopy(&newMask, linkedTo, mode);
}
rcModeUpdate(&newMask);
}
@ -118,7 +143,7 @@ bool isModeActivationConditionPresent(boxId_e modeId)
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = modeActivationConditions(i);
if (mac->modeId == modeId && IS_RANGE_USABLE(&mac->range)) {
if (mac->modeId == modeId && (IS_RANGE_USABLE(&mac->range) || mac->linkedTo)) {
return true;
}
}

2
src/main/fc/rc_modes.h
View File

@ -117,6 +117,7 @@ typedef struct modeActivationCondition_s {
uint8_t auxChannelIndex;
channelRange_t range;
modeLogic_e modeLogic;
boxId_e linkedTo;
} modeActivationCondition_t;
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
@ -129,7 +130,6 @@ typedef struct modeActivationProfile_s {
bool IS_RC_MODE_ACTIVE(boxId_e boxId);
void rcModeUpdate(boxBitmask_t *newState);
void preventModeChanges(void);
bool isAirmodeActive(void);
bool isAntiGravityModeActive(void);

31
src/main/interface/cli.c
View File

@ -848,7 +848,7 @@ static void cliRxFailsafe(char *cmdline)
static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions)
{
const char *format = "aux %u %u %u %u %u %u";
const char *format = "aux %u %u %u %u %u %u %u";
// print out aux channel settings
for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
const modeActivationCondition_t *mac = &modeActivationConditions[i];
@ -859,8 +859,10 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
&& mac->auxChannelIndex == macDefault->auxChannelIndex
&& mac->range.startStep == macDefault->range.startStep
&& mac->range.endStep == macDefault->range.endStep
&& mac->modeLogic == macDefault->modeLogic;
&& mac->modeLogic == macDefault->modeLogic
&& mac->linkedTo == macDefault->linkedTo;
const box_t *box = findBoxByBoxId(macDefault->modeId);
const box_t *linkedTo = findBoxByBoxId(macDefault->linkedTo);
if (box) {
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
@ -868,11 +870,13 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
macDefault->auxChannelIndex,
MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep),
macDefault->modeLogic
macDefault->modeLogic,
linkedTo ? linkedTo->permanentId : 0
);
}
}
const box_t *box = findBoxByBoxId(mac->modeId);
const box_t *linkedTo = findBoxByBoxId(mac->linkedTo);
if (box) {
cliDumpPrintLinef(dumpMask, equalsDefault, format,
i,
@ -880,7 +884,8 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
mac->auxChannelIndex,
MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep),
mac->modeLogic
mac->modeLogic,
linkedTo ? linkedTo->permanentId : 0
);
}
}
@ -925,18 +930,30 @@ static void cliAux(char *cmdline)
validArgumentCount++;
}
}
ptr = nextArg(ptr);
if (ptr) {
val = atoi(ptr);
const box_t *box = findBoxByPermanentId(val);
if (box) {
mac->linkedTo = box->boxId;
validArgumentCount++;
}
}
if (validArgumentCount == 4) { // for backwards compatibility
mac->modeLogic = MODELOGIC_OR;
} else if (validArgumentCount != 5) {
} else if (validArgumentCount == 5) { // for backwards compatibility
mac->linkedTo = 0;
} else if (validArgumentCount != 6) {
memset(mac, 0, sizeof(modeActivationCondition_t));
}
cliPrintLinef( "aux %u %u %u %u %u %u",
cliPrintLinef( "aux %u %u %u %u %u %u %u",
i,
mac->modeId,
mac->auxChannelIndex,
MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep),
MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep),
mac->modeLogic
mac->modeLogic,
mac->linkedTo
);
} else {
cliShowArgumentRangeError("index", 0, MAX_MODE_ACTIVATION_CONDITION_COUNT - 1);

62
src/test/unit/arming_prevention_unittest.cc
View File

@ -612,6 +612,50 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit
EXPECT_EQ(0, getArmingDisableFlags());
}
TEST(ArmingPreventionTest, ParalyzeOnAtBoot)
{
// given
simulationFeatureFlags = 0;
simulationTime = 30e6; // 30 seconds after boot
gyroCalibDone = true;
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
modeActivationConditionsMutable(1)->modeId = BOXPARALYZE;
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
useRcControlsConfig(NULL);
// and
rxConfigMutable()->mincheck = 1050;
// given
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;
rcData[AUX2] = 1800; // Paralyze on at boot
ENABLE_STATE(SMALL_ANGLE);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
// when
updateActivatedModes();
// expect
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
}
TEST(ArmingPreventionTest, Paralyze)
{
// given
@ -632,10 +676,8 @@ TEST(ArmingPreventionTest, Paralyze)
modeActivationConditionsMutable(2)->modeId = BOXBEEPERON;
modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
modeActivationConditionsMutable(3)->auxChannelIndex = 3;
modeActivationConditionsMutable(3)->modeId = BOXVTXPITMODE;
modeActivationConditionsMutable(3)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(3)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
modeActivationConditionsMutable(3)->linkedTo = BOXPARALYZE;
useRcControlsConfig(NULL);
// and
@ -646,7 +688,6 @@ TEST(ArmingPreventionTest, Paralyze)
rcData[AUX1] = 1000;
rcData[AUX2] = 1000;
rcData[AUX3] = 1000;
rcData[AUX4] = 1000;
ENABLE_STATE(SMALL_ANGLE);
// when
@ -687,9 +728,8 @@ TEST(ArmingPreventionTest, Paralyze)
EXPECT_EQ(0, getArmingDisableFlags());
// given
// paraylze and enter pit mode
// paralyze (and activate linked vtx pit mode)
rcData[AUX2] = 1800;
rcData[AUX4] = 1800;
// when
updateActivatedModes();
@ -701,12 +741,8 @@ TEST(ArmingPreventionTest, Paralyze)
EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE));
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXBEEPERON));
// and
preventModeChanges();
// given
// Try exiting pit mode and enable beeper
rcData[AUX4] = 1000;
// enable beeper
rcData[AUX3] = 1800;
// when
@ -717,7 +753,7 @@ TEST(ArmingPreventionTest, Paralyze)
EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXBEEPERON));
// given
// exit paralyze mode and ensure arming is still disabled
// try exiting paralyze mode and ensure arming and pit mode are still disabled
rcData[AUX2] = 1000;
// when
@ -727,6 +763,8 @@ TEST(ArmingPreventionTest, Paralyze)
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_PARALYZE, getArmingDisableFlags());
EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXPARALYZE));
EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXVTXPITMODE));
}
// STUBS