src/main/flight/servos.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_SERVOS
  29
  30 #include "build/build_config.h"
  31
  32 #include "common/filter.h"
  33 #include "common/maths.h"
  34
  35 #include "config/config.h"
  36 #include "config/config_reset.h"
  37 #include "config/feature.h"
  38
  39 #include "drivers/pwm_output.h"
  40
  41 #include "fc/rc_controls.h"
  42 #include "fc/rc_modes.h"
  43 #include "fc/runtime_config.h"
  44
  45 #include "flight/imu.h"
  46 #include "flight/mixer.h"
  47 #include "flight/pid.h"
  48 #include "flight/servos.h"
  49
  50 #include "io/gimbal.h"
  51
  52 #include "pg/pg.h"
  53 #include "pg/pg_ids.h"
  54 #include "pg/rx.h"
  55
  56 #include "rx/rx.h"
  57
  58
  59 PG_REGISTER_WITH_RESET_FN(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
  60
  61 void pgResetFn_servoConfig(servoConfig_t *servoConfig)
  62 {
  63     servoConfig->dev.servoCenterPulse = 1500;
  64     servoConfig->dev.servoPwmRate = 50;
  65     servoConfig->tri_unarmed_servo = 1;
  66     servoConfig->servo_lowpass_freq = 0;
  67     servoConfig->channelForwardingStartChannel = AUX1;
  68
  69 #ifdef SERVO1_PIN
  70     servoConfig->dev.ioTags[0] = IO_TAG(SERVO1_PIN);
  71 #endif
  72 #ifdef SERVO2_PIN
  73     servoConfig->dev.ioTags[1] = IO_TAG(SERVO2_PIN);
  74 #endif
  75 #ifdef SERVO3_PIN
  76     servoConfig->dev.ioTags[2] = IO_TAG(SERVO3_PIN);
  77 #endif
  78 #ifdef SERVO4_PIN
  79     servoConfig->dev.ioTags[3] = IO_TAG(SERVO4_PIN);
  80 #endif
  81 }
  82
  83 PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0);
  84
  85 PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 0);
  86
  87 void pgResetFn_servoParams(servoParam_t *instance)
  88 {
  89     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
  90         RESET_CONFIG(servoParam_t, &instance[i],
  91             .min = DEFAULT_SERVO_MIN,
  92             .max = DEFAULT_SERVO_MAX,
  93             .middle = DEFAULT_SERVO_MIDDLE,
  94             .rate = 100,
  95             .forwardFromChannel = CHANNEL_FORWARDING_DISABLED
  96         );
  97     }
  98 }
  99
 100 // no template required since default is zero
 101 PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
 102
 103 int16_t servo[MAX_SUPPORTED_SERVOS];
 104
 105 static uint8_t servoRuleCount = 0;
 106 static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
 107 static int useServo;
 108
 109
 110 #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
 111 // mixer rule format servo, input, rate, speed, min, max, box
 112 static const servoMixer_t servoMixerAirplane[] = {
 113     { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 114     { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 115     { SERVO_RUDDER,      INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 116     { SERVO_ELEVATOR,    INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 117     { SERVO_THROTTLE,    INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
 118 };
 119
 120 static const servoMixer_t servoMixerFlyingWing[] = {
 121     { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 122     { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 123     { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 },
 124     { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 125     { SERVO_THROTTLE,    INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
 126 };
 127
 128 static const servoMixer_t servoMixerTri[] = {
 129     { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 130 };
 131
 132 #if defined(USE_UNCOMMON_MIXERS)
 133 static const servoMixer_t servoMixerBI[] = {
 134     { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 135     { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, -100, 0, 0, 100, 0 },
 136     { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 137     { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 138 };
 139
 140 static const servoMixer_t servoMixerDual[] = {
 141     { SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 142     { SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 143 };
 144
 145 static const servoMixer_t servoMixerSingle[] = {
 146     { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 147     { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 148     { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 149     { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 150     { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 151     { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 152     { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 153     { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 154 };
 155
 156 static const servoMixer_t servoMixerHeli[] = {
 157     { SERVO_HELI_LEFT, INPUT_STABILIZED_PITCH,   -50, 0, 0, 100, 0 },
 158     { SERVO_HELI_LEFT, INPUT_STABILIZED_ROLL,    -87, 0, 0, 100, 0 },
 159     { SERVO_HELI_LEFT, INPUT_RC_AUX1,    100, 0, 0, 100, 0 },
 160     { SERVO_HELI_RIGHT, INPUT_STABILIZED_PITCH,  -50, 0, 0, 100, 0 },
 161     { SERVO_HELI_RIGHT, INPUT_STABILIZED_ROLL,  87, 0, 0, 100, 0 },
 162     { SERVO_HELI_RIGHT, INPUT_RC_AUX1,    100, 0, 0, 100, 0 },
 163     { SERVO_HELI_TOP, INPUT_STABILIZED_PITCH,   100, 0, 0, 100, 0 },
 164     { SERVO_HELI_TOP, INPUT_RC_AUX1,    100, 0, 0, 100, 0 },
 165     { SERVO_HELI_RUD, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
 166 };
 167 #else
 168 #define servoMixerBI NULL
 169 #define servoMixerDual NULL
 170 #define servoMixerSingle NULL
 171 #define servoMixerHeli NULL
 172 #endif // USE_UNCOMMON_MIXERS
 173
 174 static const servoMixer_t servoMixerGimbal[] = {
 175     { SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 },
 176     { SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL,  125, 0, 0, 100, 0 },
 177 };
 178
 179 const mixerRules_t servoMixers[] = {
 180     { 0, NULL },                // entry 0
 181     { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri },       // MULTITYPE_TRI
 182     { 0, NULL },                // MULTITYPE_QUADP
 183     { 0, NULL },                // MULTITYPE_QUADX
 184     { COUNT_SERVO_RULES(servoMixerBI), servoMixerBI },        // MULTITYPE_BI
 185     { COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal },    // * MULTITYPE_GIMBAL
 186     { 0, NULL },                // MULTITYPE_Y6
 187     { 0, NULL },                // MULTITYPE_HEX6
 188     { COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
 189     { 0, NULL },                // MULTITYPE_Y4
 190     { 0, NULL },                // MULTITYPE_HEX6X
 191     { 0, NULL },                // MULTITYPE_OCTOX8
 192     { 0, NULL },                // MULTITYPE_OCTOFLATP
 193     { 0, NULL },                // MULTITYPE_OCTOFLATX
 194     { COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane },  // * MULTITYPE_AIRPLANE
 195     { COUNT_SERVO_RULES(servoMixerHeli), servoMixerHeli },                // * MULTITYPE_HELI_120_CCPM
 196     { 0, NULL },                // * MULTITYPE_HELI_90_DEG
 197     { 0, NULL },                // MULTITYPE_VTAIL4
 198     { 0, NULL },                // MULTITYPE_HEX6H
 199     { 0, NULL },                // * MULTITYPE_PPM_TO_SERVO
 200     { COUNT_SERVO_RULES(servoMixerDual), servoMixerDual },      // MULTITYPE_DUALCOPTER
 201     { COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle },    // MULTITYPE_SINGLECOPTER
 202     { 0, NULL },                // MULTITYPE_ATAIL4
 203     { 0, NULL },                // MULTITYPE_CUSTOM
 204     { 0, NULL },                // MULTITYPE_CUSTOM_PLANE
 205     { 0, NULL },                // MULTITYPE_CUSTOM_TRI
 206     { 0, NULL },
 207 };
 208
 209 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
 210 {
 211     const uint8_t channelToForwardFrom = servoParams(servoIndex)->forwardFromChannel;
 212
 213     if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeState.channelCount) {
 214         return rcData[channelToForwardFrom];
 215     }
 216
 217     return servoParams(servoIndex)->middle;
 218 }
 219
 220 int servoDirection(int servoIndex, int inputSource)
 221 {
 222     // determine the direction (reversed or not) from the direction bitfield of the servo
 223     if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) {
 224         return -1;
 225     } else {
 226         return 1;
 227     }
 228 }
 229
 230 void loadCustomServoMixer(void)
 231 {
 232     // reset settings
 233     servoRuleCount = 0;
 234     memset(currentServoMixer, 0, sizeof(currentServoMixer));
 235
 236     // load custom mixer into currentServoMixer
 237     for (int i = 0; i < MAX_SERVO_RULES; i++) {
 238         // check if done
 239         if (customServoMixers(i)->rate == 0) {
 240             break;
 241         }
 242         currentServoMixer[i] = *customServoMixers(i);
 243         servoRuleCount++;
 244     }
 245 }
 246
 247 static void servoConfigureOutput(void)
 248 {
 249     if (useServo) {
 250         servoRuleCount = servoMixers[getMixerMode()].servoRuleCount;
 251         if (servoMixers[getMixerMode()].rule) {
 252             for (int i = 0; i < servoRuleCount; i++)
 253                 currentServoMixer[i] = servoMixers[getMixerMode()].rule[i];
 254         }
 255     }
 256
 257     switch (getMixerMode()) {
 258     case MIXER_CUSTOM_AIRPLANE:
 259     case MIXER_CUSTOM_TRI:
 260         loadCustomServoMixer();
 261         break;
 262     default:
 263         break;
 264     }
 265 }
 266
 267
 268 void servosInit(void)
 269 {
 270     // enable servos for mixes that require them. note, this shifts motor counts.
 271     useServo = mixers[getMixerMode()].useServo;
 272     // if we want camstab/trig, that also enables servos, even if mixer doesn't
 273     if (featureIsEnabled(FEATURE_SERVO_TILT) || featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
 274         useServo = 1;
 275     }
 276
 277     // give all servos a default command
 278     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 279         servo[i] = DEFAULT_SERVO_MIDDLE;
 280     }
 281
 282     if (mixerIsTricopter()) {
 283         servosTricopterInit();
 284     }
 285
 286     servoConfigureOutput();
 287 }
 288
 289 void servoMixerLoadMix(int index)
 290 {
 291     // we're 1-based
 292     index++;
 293     // clear existing
 294     for (int i = 0; i < MAX_SERVO_RULES; i++) {
 295         customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = customServoMixersMutable(i)->box = 0;
 296     }
 297     for (int i = 0; i < servoMixers[index].servoRuleCount; i++) {
 298         *customServoMixersMutable(i) = servoMixers[index].rule[i];
 299     }
 300 }
 301
 302 STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
 303 {
 304     // start forwarding from this channel
 305     int channelOffset = servoConfig()->channelForwardingStartChannel;
 306     const int maxAuxChannelCount = MIN(MAX_AUX_CHANNEL_COUNT, rxConfig()->max_aux_channel);
 307     for (int servoOffset = 0; servoOffset < maxAuxChannelCount && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
 308         pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
 309     }
 310 }
 311
 312 // Write and keep track of written servos
 313
 314 static uint32_t servoWritten;
 315
 316 STATIC_ASSERT(sizeof(servoWritten) * 8 >= MAX_SUPPORTED_SERVOS, servoWritten_is_too_small);
 317
 318 static void writeServoWithTracking(uint8_t index, servoIndex_e servoname)
 319 {
 320     pwmWriteServo(index, servo[servoname]);
 321     servoWritten |= (1 << servoname);
 322 }
 323
 324 static void updateGimbalServos(uint8_t firstServoIndex)
 325 {
 326     writeServoWithTracking(firstServoIndex + 0, SERVO_GIMBAL_PITCH);
 327     writeServoWithTracking(firstServoIndex + 1, SERVO_GIMBAL_ROLL);
 328 }
 329
 330 static void servoTable(void);
 331 static void filterServos(void);
 332
 333 void writeServos(void)
 334 {
 335     servoTable();
 336     filterServos();
 337
 338     uint8_t servoIndex = 0;
 339     switch (getMixerMode()) {
 340     case MIXER_TRI:
 341     case MIXER_CUSTOM_TRI:
 342         // We move servo if unarmed flag set or armed
 343         if (!(servosTricopterIsEnabledServoUnarmed() || ARMING_FLAG(ARMED))) {
 344             servo[SERVO_RUDDER] = 0; // kill servo signal completely.
 345         }
 346         writeServoWithTracking(servoIndex++, SERVO_RUDDER);
 347         break;
 348
 349     case MIXER_FLYING_WING:
 350         writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_1);
 351         writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_2);
 352         break;
 353
 354     case MIXER_CUSTOM_AIRPLANE:
 355     case MIXER_AIRPLANE:
 356         for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
 357             writeServoWithTracking(servoIndex++, i);
 358         }
 359         break;
 360
 361 #ifdef USE_UNCOMMON_MIXERS
 362     case MIXER_BICOPTER:
 363         writeServoWithTracking(servoIndex++, SERVO_BICOPTER_LEFT);
 364         writeServoWithTracking(servoIndex++, SERVO_BICOPTER_RIGHT);
 365         break;
 366
 367     case MIXER_HELI_120_CCPM:
 368         writeServoWithTracking(servoIndex++, SERVO_HELI_LEFT);
 369         writeServoWithTracking(servoIndex++, SERVO_HELI_RIGHT);
 370         writeServoWithTracking(servoIndex++, SERVO_HELI_TOP);
 371         writeServoWithTracking(servoIndex++, SERVO_HELI_RUD);
 372         break;
 373
 374     case MIXER_DUALCOPTER:
 375         writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_LEFT);
 376         writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_RIGHT);
 377         break;
 378
 379     case MIXER_SINGLECOPTER:
 380         for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
 381             writeServoWithTracking(servoIndex++, i);
 382         }
 383         break;
 384 #endif // USE_UNCOMMON_MIXERS
 385
 386     default:
 387         break;
 388     }
 389
 390     // Two servos for SERVO_TILT, if enabled
 391     if (featureIsEnabled(FEATURE_SERVO_TILT) || getMixerMode() == MIXER_GIMBAL) {
 392         updateGimbalServos(servoIndex);
 393         servoIndex += 2;
 394     }
 395
 396     // Scan servos and write those marked forwarded and not written yet
 397     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 398         const uint8_t channelToForwardFrom = servoParams(i)->forwardFromChannel;
 399         if ((channelToForwardFrom != CHANNEL_FORWARDING_DISABLED) && !(servoWritten & (1 << i))) {
 400             pwmWriteServo(servoIndex++, servo[i]);
 401         }
 402     }
 403
 404     // forward AUX to remaining servo outputs (not constrained)
 405     if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
 406         forwardAuxChannelsToServos(servoIndex);
 407         servoIndex += MAX_AUX_CHANNEL_COUNT;
 408     }
 409 }
 410
 411 void servoMixer(void)
 412 {
 413     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
 414     static int16_t currentOutput[MAX_SERVO_RULES];
 415
 416     if (FLIGHT_MODE(PASSTHRU_MODE)) {
 417         // Direct passthru from RX
 418         input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
 419         input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
 420         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
 421     } else {
 422         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
 423         input[INPUT_STABILIZED_ROLL] = pidData[FD_ROLL].Sum * PID_SERVO_MIXER_SCALING;
 424         input[INPUT_STABILIZED_PITCH] = pidData[FD_PITCH].Sum * PID_SERVO_MIXER_SCALING;
 425         input[INPUT_STABILIZED_YAW] = pidData[FD_YAW].Sum * PID_SERVO_MIXER_SCALING;
 426
 427         // Reverse yaw servo when inverted in 3D mode
 428         if (featureIsEnabled(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {
 429             input[INPUT_STABILIZED_YAW] *= -1;
 430         }
 431     }
 432
 433     input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
 434     input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
 435
 436     input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]
 437
 438     // center the RC input value around the RC middle value
 439     // by subtracting the RC middle value from the RC input value, we get:
 440     // data - middle = input
 441     // 2000 - 1500 = +500
 442     // 1500 - 1500 = 0
 443     // 1000 - 1500 = -500
 444     input[INPUT_RC_ROLL]     = rcData[ROLL]     - rxConfig()->midrc;
 445     input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig()->midrc;
 446     input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig()->midrc;
 447     input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc;
 448     input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig()->midrc;
 449     input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig()->midrc;
 450     input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig()->midrc;
 451     input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig()->midrc;
 452
 453     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 454         servo[i] = 0;
 455     }
 456
 457     // mix servos according to rules
 458     for (int i = 0; i < servoRuleCount; i++) {
 459         // consider rule if no box assigned or box is active
 460         if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
 461             uint8_t target = currentServoMixer[i].targetChannel;
 462             uint8_t from = currentServoMixer[i].inputSource;
 463             uint16_t servo_width = servoParams(target)->max - servoParams(target)->min;
 464             int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
 465             int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
 466
 467             if (currentServoMixer[i].speed == 0)
 468                 currentOutput[i] = input[from];
 469             else {
 470                 if (currentOutput[i] < input[from])
 471                     currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
 472                 else if (currentOutput[i] > input[from])
 473                     currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
 474             }
 475
 476             servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
 477         } else {
 478             currentOutput[i] = 0;
 479         }
 480     }
 481
 482     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 483         servo[i] = ((int32_t)servoParams(i)->rate * servo[i]) / 100L;
 484         servo[i] += determineServoMiddleOrForwardFromChannel(i);
 485     }
 486 }
 487
 488
 489 static void servoTable(void)
 490 {
 491     // airplane / servo mixes
 492     switch (getMixerMode()) {
 493     case MIXER_CUSTOM_TRI:
 494     case MIXER_TRI:
 495         servosTricopterMixer();
 496         break;
 497     case MIXER_CUSTOM_AIRPLANE:
 498     case MIXER_FLYING_WING:
 499     case MIXER_AIRPLANE:
 500     case MIXER_BICOPTER:
 501     case MIXER_DUALCOPTER:
 502     case MIXER_SINGLECOPTER:
 503     case MIXER_HELI_120_CCPM:
 504     case MIXER_GIMBAL:
 505         servoMixer();
 506         break;
 507
 508     /*
 509     case MIXER_GIMBAL:
 510         servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
 511         servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
 512         break;
 513     */
 514
 515     default:
 516         break;
 517     }
 518
 519     // camera stabilization
 520     if (featureIsEnabled(FEATURE_SERVO_TILT)) {
 521         // center at fixed position, or vary either pitch or roll by RC channel
 522         servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
 523         servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
 524
 525         if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
 526             if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) {
 527                 servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 - (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50;
 528                 servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 + (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50;
 529             } else {
 530                 servo[SERVO_GIMBAL_PITCH] += (int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch / 50;
 531                 servo[SERVO_GIMBAL_ROLL] += (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll  / 50;
 532             }
 533         }
 534     }
 535
 536     // constrain servos
 537     for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 538         servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); // limit the values
 539     }
 540 }
 541
 542 bool isMixerUsingServos(void)
 543 {
 544     return useServo;
 545 }
 546
 547 static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
 548
 549 void servosFilterInit(void)
 550 {
 551     if (servoConfig()->servo_lowpass_freq) {
 552         for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
 553             biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
 554         }
 555     }
 556
 557 }
 558 static void filterServos(void)
 559 {
 560 #if defined(MIXER_DEBUG)
 561     uint32_t startTime = micros();
 562 #endif
 563     if (servoConfig()->servo_lowpass_freq) {
 564         for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
 565             servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
 566             // Sanity check
 567             servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
 568         }
 569     }
 570 #if defined(MIXER_DEBUG)
 571     debug[0] = (int16_t)(micros() - startTime);
 572 #endif
 573 }
 574 #endif // USE_SERVOS