From 394a28824f22e8dd8ef0dd970eee0e8f0d487ab8 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Mon, 25 Jul 2016 15:06:31 +0300 Subject: [PATCH] Allow ANDing of multiple channel ranges in mode activation conditions (#364) * Allow ANDing of multiple channel ranges in mode activation conditions (CLI: set and_mode_conditions=ON) * Moved mode logic operator to Profile (modeActivationConditions are part of profile already); Created a lookup table for logic operator * Closes #212 --- src/main/config/config.c | 2 ++ src/main/config/config_profile.h | 1 + src/main/io/rc_controls.c | 54 +++++++++++++++++++++++++++++++++------- src/main/io/rc_controls.h | 7 +++++- src/main/io/serial_cli.c | 7 ++++++ src/main/mw.c | 2 +- 6 files changed, 62 insertions(+), 11 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1446068a9..073c8416a 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -578,6 +578,8 @@ static void resetConf(void) currentProfile->mag_declination = 0; + currentProfile->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions + resetBarometerConfig(&masterConfig.barometerConfig); // Radio diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 713a0e920..50f9e6608 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -26,6 +26,7 @@ typedef struct profile_s { // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + modeActivationOperator_e modeActivationOperator; adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c83a69d81..2e73fd40b 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -28,9 +28,6 @@ #include "common/axis.h" #include "common/maths.h" -#include "config/config.h" -#include "config/runtime_config.h" - #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -55,6 +52,9 @@ #include "flight/navigation_rewrite.h" #include "flight/failsafe.h" +#include "config/config.h" +#include "config/runtime_config.h" + #include "blackbox/blackbox.h" #include "mw.h" @@ -303,17 +303,53 @@ bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) { channelValue < 900 + (range->endStep * 25)); } -void updateActivatedModes(modeActivationCondition_t *modeActivationConditions) +void updateActivatedModes(modeActivationCondition_t *modeActivationConditions, modeActivationOperator_e modeActivationOperator) { - rcModeActivationMask = 0; + uint8_t modeIndex; - uint8_t index; + // Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode. + // We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid. - for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; + uint8_t validConditionCountPerMode[CHECKBOX_ITEM_COUNT]; + + memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT); + memset(validConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT); + + for (modeIndex = 0; modeIndex < MAX_MODE_ACTIVATION_CONDITION_COUNT; modeIndex++) { + modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[modeIndex]; + + // Increment the number of specified conditions for this mode + specifiedConditionCountPerMode[modeActivationCondition->modeId]++; if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { - ACTIVATE_RC_MODE(modeActivationCondition->modeId); + // Increment the number of valid conditions for this mode + validConditionCountPerMode[modeActivationCondition->modeId]++; + } + } + + // Disable all modes to begin with + rcModeActivationMask = 0; + + // Now see which modes should be enabled + for (modeIndex = 0; modeIndex < CHECKBOX_ITEM_COUNT; modeIndex++) { + // only modes with conditions specified are considered + if (specifiedConditionCountPerMode[modeIndex] > 0) { + // For AND logic, the specified condition count and valid condition count must be the same. + // For OR logic, the valid condition count must be greater than zero. + + if (modeActivationOperator == MODE_OPERATOR_AND) { + // AND the conditions + if (validConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) { + ACTIVATE_RC_MODE(modeIndex); + } + } + else { + // OR the conditions + if (validConditionCountPerMode[modeIndex] > 0) { + ACTIVATE_RC_MODE(modeIndex); + } + } } } } diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index bdc990613..4dcec7aa7 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -144,6 +144,11 @@ typedef struct modeActivationCondition_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) +typedef enum { + MODE_OPERATOR_OR, + MODE_OPERATOR_AND +} modeActivationOperator_e; + typedef struct controlRateConfig_s { uint8_t rcExpo8; uint8_t thrMid8; @@ -170,7 +175,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig); void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); -void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); +void updateActivatedModes(modeActivationCondition_t *modeActivationConditions, modeActivationOperator_e modeActivationOperator); typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index fa800c4b1..3aa1b562c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -423,6 +423,10 @@ static const char * const lookupTableNavRthAltMode[] = { }; #endif +static const char * const lookupTableAuxOperator[] = { + "OR", "AND" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -457,6 +461,7 @@ typedef enum { TABLE_NAV_USER_CTL_MODE, TABLE_NAV_RTH_ALT_MODE, #endif + TABLE_AUX_OPERATOR, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -488,6 +493,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableNavControlMode, sizeof(lookupTableNavControlMode) / sizeof(char *) }, { lookupTableNavRthAltMode, sizeof(lookupTableNavRthAltMode) / sizeof(char *) }, #endif + { lookupTableAuxOperator, sizeof(lookupTableAuxOperator) / sizeof(char *) }, }; #define VALUE_TYPE_OFFSET 0 @@ -725,6 +731,7 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, #endif + { "mode_range_logic_operator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].modeActivationOperator, .config.lookup = { TABLE_AUX_OPERATOR }, 0 }, { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, 0 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, .config.minmax = { 0, 100 }, 0 }, { "rc_yaw_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcYawExpo8, .config.minmax = { 0, 100 }, 0 }, diff --git a/src/main/mw.c b/src/main/mw.c index 85d1e9b58..4a6c91c6a 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -336,7 +336,7 @@ void processRx(void) processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch); - updateActivatedModes(currentProfile->modeActivationConditions); + updateActivatedModes(currentProfile->modeActivationConditions, currentProfile->modeActivationOperator); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); -- 2.11.4.GIT