diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 40711706e78..51f362e44ca 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -106,7 +106,6 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; - /* //Was used to keep track of servo rules in all mixer_profile, In order to Apply mixer speed limit when rules turn off static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; @@ -207,6 +206,25 @@ int getServoCount(void) void loadCustomServoMixer(void) { + + //move the rate filter to new servo rules + int movefilterCount = 0; + static servoMixerSwitch_t servoMixerSwitchHelper[MAX_SERVO_RULES_SWITCH_CARRY]; // helper to keep track of servoSpeedLimitFilter of servo rules + memset(servoMixerSwitchHelper, 0, sizeof(servoMixerSwitchHelper)); + for (int i = 0; i < servoRuleCount; i++) { + if(currentServoMixer[i].inputSource == INPUT_MIXER_SWITCH_HELPER || movefilterCount >= MAX_SERVO_RULES_SWITCH_CARRY) { + //will not carry over INPUT_MIXER_SWITCH_HELPER rules + break; + } + if(currentServoMixer[i].speed != 0 && fabsf(servoSpeedLimitFilter[i].state) > 0.01f) { + servoMixerSwitchHelper[movefilterCount].targetChannel = currentServoMixer[i].targetChannel; + servoMixerSwitchHelper[movefilterCount].speed = currentServoMixer[i].speed; + servoMixerSwitchHelper[movefilterCount].rate = currentServoMixer[i].rate; + servoMixerSwitchHelper[movefilterCount].speedLimitFilterState = servoSpeedLimitFilter[i].state; + movefilterCount++; + } + } + servoRuleCount = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); @@ -220,6 +238,19 @@ void loadCustomServoMixer(void) servoSpeedLimitFilter[servoRuleCount].state = 0; servoRuleCount++; } + + // add servo rules to handle the rate limit filter + for (int i = 0; i < movefilterCount; i++) { + if (servoRuleCount >= MAX_SERVO_RULES) { + break; // prevent overflow + } + currentServoMixer[servoRuleCount].targetChannel = servoMixerSwitchHelper[i].targetChannel; + currentServoMixer[servoRuleCount].speed = servoMixerSwitchHelper[i].speed; + currentServoMixer[servoRuleCount].rate = servoMixerSwitchHelper[i].rate; + currentServoMixer[servoRuleCount].inputSource = INPUT_MIXER_SWITCH_HELPER; // no input + servoSpeedLimitFilter[servoRuleCount].state = servoMixerSwitchHelper[i].speedLimitFilterState; + servoRuleCount++; + } } static void filterServos(void) @@ -323,6 +354,7 @@ void servoMixer(float dT) input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 500; //fixed value + input[INPUT_MIXER_SWITCH_HELPER] = 0; // no input, used to apply speed limit filter from previous servo rules // center the RC input value around the RC middle value // by subtracting the RC middle value from the RC input value, we get: diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 23ac3fda495..3e3b087c3db 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -84,6 +84,7 @@ typedef enum { INPUT_RC_CH32 = 57, INPUT_RC_CH33 = 58, INPUT_RC_CH34 = 59, + INPUT_MIXER_SWITCH_HELPER = 60, INPUT_SOURCE_COUNT } inputSource_e; @@ -142,6 +143,15 @@ typedef struct servoMixer_s { PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers); +typedef struct servoMixerSwitch_s { + //this is used to keep track of servoSpeedLimitFilter of servo rules during the mixer switch + uint8_t targetChannel; // servo that receives the output of the rule + int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + float speedLimitFilterState; // rate limit filter for this rule +} servoMixerSwitch_t; +#define MAX_SERVO_RULES_SWITCH_CARRY (MAX_SERVO_RULES / 2) + typedef struct servoParam_s { int16_t min; // servo min int16_t max; // servo max