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