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