diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index c6864094cc0..d0585a023f9 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -61,7 +61,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .rate = 0, .speed = 0 #ifdef USE_PROGRAMMING_FRAMEWORK - ,.conditionId = -1 + ,.conditionId = -1, #endif ); } @@ -98,34 +98,34 @@ static int computeMotorCountByMixerProfileIndex(int index) return motorCount; } -static int computeServoCountByMixerProfileIndex(int index) -{ - int servoRuleCount = 0; - int minServoIndex = 255; - int maxServoIndex = 0; - - const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; - for (int i = 0; i < MAX_SERVO_RULES; i++) { - if (temp_servomixers[i].rate == 0) - break; - - if (temp_servomixers[i].targetChannel < minServoIndex) { - minServoIndex = temp_servomixers[i].targetChannel; - } - - if (temp_servomixers[i].targetChannel > maxServoIndex) { - maxServoIndex = temp_servomixers[i].targetChannel; - } - // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); - servoRuleCount++; - } - if (servoRuleCount) { - return 1 + maxServoIndex - minServoIndex; - } - else { - return 0; - } -} +// static int computeServoCountByMixerProfileIndex(int index) +// { +// int servoRuleCount = 0; +// int minServoIndex = 255; +// int maxServoIndex = 0; + +// const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; +// for (int i = 0; i < MAX_SERVO_RULES; i++) { +// if (temp_servomixers[i].rate == 0) +// break; + +// if (temp_servomixers[i].targetChannel < minServoIndex) { +// minServoIndex = temp_servomixers[i].targetChannel; +// } + +// if (temp_servomixers[i].targetChannel > maxServoIndex) { +// maxServoIndex = temp_servomixers[i].targetChannel; +// } +// // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); +// servoRuleCount++; +// } +// if (servoRuleCount) { +// return 1 + maxServoIndex - minServoIndex; +// } +// else { +// return 0; +// } +// } bool checkMixerProfileHotSwitchAvalibility(void) { @@ -160,7 +160,8 @@ bool checkMixerProfileHotSwitchAvalibility(void) return false; } //do not allow switching if motor or servos counts are different - if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) + // if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) + if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1))) { allow_hot_switch = 0; return false; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 45d32bf7ff5..4b421d41180 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -120,7 +120,8 @@ void pgResetFn_servoParams(servoParam_t *instance) int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; -static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; +static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer static bool servoOutputEnabled; static bool mixerUsesServos; @@ -192,28 +193,31 @@ int getServoCount(void) void loadCustomServoMixer(void) { - // reset settings servoRuleCount = 0; minServoIndex = 255; maxServoIndex = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); - // load custom mixer into currentServoMixer - for (int i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (customServoMixers(i)->rate == 0) - break; + for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { + const servoMixer_t* tmp_customServoMixers = mixerServoMixersByIndex(j)[0]; + // load custom mixer into currentServoMixer + for (int i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (tmp_customServoMixers[i].rate == 0) + break; - if (customServoMixers(i)->targetChannel < minServoIndex) { - minServoIndex = customServoMixers(i)->targetChannel; - } + if (tmp_customServoMixers[i].targetChannel < minServoIndex) { + minServoIndex = tmp_customServoMixers[i].targetChannel; + } - if (customServoMixers(i)->targetChannel > maxServoIndex) { - maxServoIndex = customServoMixers(i)->targetChannel; - } + if (tmp_customServoMixers[i].targetChannel > maxServoIndex) { + maxServoIndex = tmp_customServoMixers[i].targetChannel; + } - memcpy(¤tServoMixer[i], customServoMixers(i), sizeof(servoMixer_t)); - servoRuleCount++; + memcpy(¤tServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t)); + currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex; + servoRuleCount++; + } } } @@ -357,19 +361,22 @@ void servoMixer(float dT) // mix servos according to rules for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + + int16_t inputRaw = input[from]; /* * Check if conditions for a rule are met, not all conditions apply all the time */ #ifdef USE_PROGRAMMING_FRAMEWORK if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { - continue; + inputRaw = 0; } #endif - - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - + if (!currentServoMixerActivative[i]) { + inputRaw = 0; + } /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -377,7 +384,7 @@ void servoMixer(float dT) * 10 = 100us/s -> full sweep (from 1000 to 2000) is performed in 10s * 100 = 1000us/s -> full sweep in 1s */ - int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i+MAX_SERVO_RULES*currentMixerProfileIndex], input[from], currentServoMixer[i].speed * 10, dT); + int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], inputRaw, currentServoMixer[i].speed * 10, dT); servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } @@ -452,6 +459,8 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} + // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -473,6 +482,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -485,6 +495,7 @@ void processServoAutotrimMode(void) if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -518,6 +529,7 @@ void processServoAutotrimMode(void) if (trimState == AUTOTRIM_SAVE_PENDING) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) {