src/main/flight/mixer.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include "platform.h"
  23
  24 #include "build_config.h"
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28
  29 #include "drivers/gpio.h"
  30 #include "drivers/timer.h"
  31 #include "drivers/pwm_output.h"
  32 #include "drivers/pwm_mapping.h"
  33 #include "drivers/sensor.h"
  34 #include "drivers/accgyro.h"
  35
  36 #include "rx/rx.h"
  37
  38 #include "io/gimbal.h"
  39 #include "io/escservo.h"
  40 #include "io/rc_controls.h"
  41
  42 #include "sensors/sensors.h"
  43 #include "sensors/acceleration.h"
  44
  45 #include "flight/mixer.h"
  46 #include "flight/pid.h"
  47 #include "flight/imu.h"
  48
  49 #include "config/runtime_config.h"
  50 #include "config/config.h"
  51
  52 #define GIMBAL_SERVO_PITCH 0
  53 #define GIMBAL_SERVO_ROLL 1
  54
  55 #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
  56
  57 uint8_t motorCount = 0;
  58 int16_t motor[MAX_SUPPORTED_MOTORS];
  59 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
  60 int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
  61
  62 static int useServo;
  63
  64 static uint8_t servoCount;
  65
  66 static servoParam_t *servoConf;
  67 static mixerConfig_t *mixerConfig;
  68 static flight3DConfig_t *flight3DConfig;
  69 static escAndServoConfig_t *escAndServoConfig;
  70 static airplaneConfig_t *airplaneConfig;
  71 static rxConfig_t *rxConfig;
  72 static gimbalConfig_t *gimbalConfig;
  73
  74 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  75 static mixerMode_e currentMixerMode;
  76
  77 static const motorMixer_t mixerQuadX[] = {
  78     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
  79     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
  80     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
  81     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
  82 };
  83 #ifndef USE_QUAD_MIXER_ONLY
  84 static const motorMixer_t mixerTri[] = {
  85     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
  86     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
  87     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
  88 };
  89
  90 static const motorMixer_t mixerQuadP[] = {
  91     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
  92     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
  93     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
  94     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
  95 };
  96
  97 static const motorMixer_t mixerBi[] = {
  98     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
  99     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
 100 };
 101
 102 static const motorMixer_t mixerY6[] = {
 103     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 104     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 105     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 106     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 107     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 108     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 109 };
 110
 111 static const motorMixer_t mixerHex6P[] = {
 112     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 113     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 114     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 115     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 116     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 117     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 118 };
 119
 120 static const motorMixer_t mixerY4[] = {
 121     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
 122     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
 123     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
 124     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
 125 };
 126
 127 static const motorMixer_t mixerHex6X[] = {
 128     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 129     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 130     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 131     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 132     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 133     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 134 };
 135
 136 static const motorMixer_t mixerOctoX8[] = {
 137     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 138     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 139     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 140     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 141     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 142     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 143     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 144     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 145 };
 146
 147 static const motorMixer_t mixerOctoFlatP[] = {
 148     { 1.0f,  0.707107f, -0.707107f,  1.0f },    // FRONT_L
 149     { 1.0f, -0.707107f, -0.707107f,  1.0f },    // FRONT_R
 150     { 1.0f, -0.707107f,  0.707107f,  1.0f },    // REAR_R
 151     { 1.0f,  0.707107f,  0.707107f,  1.0f },    // REAR_L
 152     { 1.0f,  0.0f, -1.0f, -1.0f },              // FRONT
 153     { 1.0f, -1.0f,  0.0f, -1.0f },              // RIGHT
 154     { 1.0f,  0.0f,  1.0f, -1.0f },              // REAR
 155     { 1.0f,  1.0f,  0.0f, -1.0f },              // LEFT
 156 };
 157
 158 static const motorMixer_t mixerOctoFlatX[] = {
 159     { 1.0f,  1.0f, -0.5f,  1.0f },          // MIDFRONT_L
 160     { 1.0f, -0.5f, -1.0f,  1.0f },          // FRONT_R
 161     { 1.0f, -1.0f,  0.5f,  1.0f },          // MIDREAR_R
 162     { 1.0f,  0.5f,  1.0f,  1.0f },          // REAR_L
 163     { 1.0f,  0.5f, -1.0f, -1.0f },          // FRONT_L
 164     { 1.0f, -1.0f, -0.5f, -1.0f },          // MIDFRONT_R
 165     { 1.0f, -0.5f,  1.0f, -1.0f },          // REAR_R
 166     { 1.0f,  1.0f,  0.5f, -1.0f },          // MIDREAR_L
 167 };
 168
 169 static const motorMixer_t mixerVtail4[] = {
 170     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_R
 171     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R
 172     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_L
 173     { 1.0f,  1.0f, -1.0f, -0.0f },          // FRONT_L
 174 };
 175
 176 static const motorMixer_t mixerAtail4[] = {
 177     { 1.0f, 0.0f, 1.0f, 1.0f },             // REAR_R
 178     { 1.0f, -1.0f, -1.0f, 0.0f },           // FRONT_R
 179     { 1.0f, 0.0f, 1.0f, -1.0f },            // REAR_L
 180     { 1.0f, 1.0f, -1.0f, -0.0f },           // FRONT_L
 181 };
 182
 183 static const motorMixer_t mixerHex6H[] = {
 184     { 1.0f, -1.0f,  1.0f, -1.0f },     // REAR_R
 185     { 1.0f, -1.0f, -1.0f,  1.0f },     // FRONT_R
 186     { 1.0f,  1.0f,  1.0f,  1.0f },     // REAR_L
 187     { 1.0f,  1.0f, -1.0f, -1.0f },     // FRONT_L
 188     { 1.0f,  0.0f,  0.0f,  0.0f },     // RIGHT
 189     { 1.0f,  0.0f,  0.0f,  0.0f },     // LEFT
 190 };
 191
 192 static const motorMixer_t mixerDualcopter[] = {
 193     { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 194     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 195 };
 196
 197 // Keep this synced with MultiType struct in mw.h!
 198 const mixer_t mixers[] = {
 199 //    Mo Se Mixtable
 200     { 0, 0, NULL },                // entry 0
 201     { 3, 1, mixerTri },            // MIXER_TRI
 202     { 4, 0, mixerQuadP },          // MIXER_QUADP
 203     { 4, 0, mixerQuadX },          // MIXER_QUADX
 204     { 2, 1, mixerBi },             // MIXER_BI
 205     { 0, 1, NULL },                // * MIXER_GIMBAL
 206     { 6, 0, mixerY6 },             // MIXER_Y6
 207     { 6, 0, mixerHex6P },          // MIXER_HEX6
 208     { 1, 1, NULL },                // * MIXER_FLYING_WING
 209     { 4, 0, mixerY4 },             // MIXER_Y4
 210     { 6, 0, mixerHex6X },          // MIXER_HEX6X
 211     { 8, 0, mixerOctoX8 },         // MIXER_OCTOX8
 212     { 8, 0, mixerOctoFlatP },      // MIXER_OCTOFLATP
 213     { 8, 0, mixerOctoFlatX },      // MIXER_OCTOFLATX
 214     { 1, 1, NULL },                // * MIXER_AIRPLANE
 215     { 0, 1, NULL },                // * MIXER_HELI_120_CCPM
 216     { 0, 1, NULL },                // * MIXER_HELI_90_DEG
 217     { 4, 0, mixerVtail4 },         // MIXER_VTAIL4
 218     { 6, 0, mixerHex6H },          // MIXER_HEX6H
 219     { 0, 1, NULL },                // * MIXER_PPM_TO_SERVO
 220     { 2, 1, mixerDualcopter  },    // MIXER_DUALCOPTER
 221     { 1, 1, NULL },                // MIXER_SINGLECOPTER
 222     { 4, 0, mixerAtail4 },         // MIXER_ATAIL4
 223     { 0, 0, NULL },                // MIXER_CUSTOM
 224 };
 225 #endif
 226
 227 void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse)
 228 {
 229     servoConf = servoConfToUse;
 230     flight3DConfig = flight3DConfigToUse;
 231     escAndServoConfig = escAndServoConfigToUse;
 232     mixerConfig = mixerConfigToUse;
 233     airplaneConfig = airplaneConfigToUse;
 234     rxConfig = rxConfigToUse;
 235     gimbalConfig = gimbalConfigToUse;
 236 }
 237
 238 int16_t determineServoMiddleOrForwardFromChannel(int nr)
 239 {
 240     uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
 241
 242     if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
 243         return rcData[channelToForwardFrom];
 244     }
 245
 246     if (nr < MAX_SUPPORTED_SERVOS) {
 247         return servoConf[nr].middle;
 248     }
 249
 250     return DEFAULT_SERVO_MIDDLE;
 251 }
 252
 253 int servoDirection(int nr, int lr)
 254 {
 255     // servo.rate is overloaded for servos that don't have a rate, but only need direction
 256     // bit set = negative, clear = positive
 257     // rate[2] = ???_direction
 258     // rate[1] = roll_direction
 259     // rate[0] = pitch_direction
 260     // servo.rate is also used as gimbal gain multiplier (yeah)
 261     if (servoConf[nr].rate & lr)
 262         return -1;
 263     else
 264         return 1;
 265 }
 266
 267 static motorMixer_t *customMixers;
 268
 269
 270 #ifndef USE_QUAD_MIXER_ONLY
 271 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
 272 {
 273     currentMixerMode = mixerMode;
 274
 275     customMixers = initialCustomMixers;
 276
 277     // enable servos for mixes that require them. note, this shifts motor counts.
 278     useServo = mixers[currentMixerMode].useServo;
 279     // if we want camstab/trig, that also enables servos, even if mixer doesn't
 280     if (feature(FEATURE_SERVO_TILT))
 281         useServo = 1;
 282 }
 283
 284 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
 285 {
 286     int i;
 287
 288     servoCount = pwmOutputConfiguration->servoCount;
 289
 290     if (currentMixerMode == MIXER_CUSTOM) {
 291         // load custom mixer into currentMixer
 292         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 293             // check if done
 294             if (customMixers[i].throttle == 0.0f)
 295                 break;
 296             currentMixer[i] = customMixers[i];
 297             motorCount++;
 298         }
 299     } else {
 300         motorCount = mixers[currentMixerMode].motorCount;
 301         // copy motor-based mixers
 302         if (mixers[currentMixerMode].motor) {
 303             for (i = 0; i < motorCount; i++)
 304                 currentMixer[i] = mixers[currentMixerMode].motor[i];
 305         }
 306     }
 307
 308     // in 3D mode, mixer gain has to be halved
 309     if (feature(FEATURE_3D)) {
 310         if (motorCount > 1) {
 311             for (i = 0; i < motorCount; i++) {
 312                 currentMixer[i].pitch *= 0.5f;
 313                 currentMixer[i].roll *= 0.5f;
 314                 currentMixer[i].yaw *= 0.5f;
 315             }
 316         }
 317     }
 318
 319     // set flag that we're on something with wings
 320     if (currentMixerMode == MIXER_FLYING_WING ||
 321             currentMixerMode == MIXER_AIRPLANE)
 322         ENABLE_STATE(FIXED_WING);
 323     else
 324         DISABLE_STATE(FIXED_WING);
 325
 326     mixerResetMotors();
 327 }
 328
 329 void mixerLoadMix(int index, motorMixer_t *customMixers)
 330 {
 331     int i;
 332
 333     // we're 1-based
 334     index++;
 335     // clear existing
 336     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 337         customMixers[i].throttle = 0.0f;
 338
 339     // do we have anything here to begin with?
 340     if (mixers[index].motor != NULL) {
 341         for (i = 0; i < mixers[index].motorCount; i++)
 342             customMixers[i] = mixers[index].motor[i];
 343     }
 344 }
 345
 346 #else
 347
 348 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
 349 {
 350     currentMixerMode = mixerMode;
 351
 352     customMixers = initialCustomMixers;
 353
 354     useServo = false;
 355 }
 356
 357 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
 358 {
 359     UNUSED(pwmOutputConfiguration);
 360     motorCount = 4;
 361     servoCount = 0;
 362
 363     uint8_t i;
 364     for (i = 0; i < motorCount; i++) {
 365         currentMixer[i] = mixerQuadX[i];
 366     }
 367
 368     mixerResetMotors();
 369 }
 370 #endif
 371
 372 void mixerResetMotors(void)
 373 {
 374     int i;
 375     // set disarmed motor values
 376     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 377         motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
 378 }
 379
 380 static void updateGimbalServos(void)
 381 {
 382     pwmWriteServo(0, servo[0]);
 383     pwmWriteServo(1, servo[1]);
 384 }
 385
 386 void writeServos(void)
 387 {
 388     if (!useServo)
 389         return;
 390
 391     switch (currentMixerMode) {
 392         case MIXER_BI:
 393             pwmWriteServo(0, servo[4]);
 394             pwmWriteServo(1, servo[5]);
 395             break;
 396
 397         case MIXER_TRI:
 398             if (mixerConfig->tri_unarmed_servo) {
 399                 // if unarmed flag set, we always move servo
 400                 pwmWriteServo(0, servo[5]);
 401             } else {
 402                 // otherwise, only move servo when copter is armed
 403                 if (ARMING_FLAG(ARMED))
 404                     pwmWriteServo(0, servo[5]);
 405                 else
 406                     pwmWriteServo(0, 0); // kill servo signal completely.
 407             }
 408             break;
 409
 410         case MIXER_FLYING_WING:
 411             pwmWriteServo(0, servo[3]);
 412             pwmWriteServo(1, servo[4]);
 413             break;
 414
 415         case MIXER_GIMBAL:
 416             updateGimbalServos();
 417             break;
 418
 419         case MIXER_DUALCOPTER:
 420             pwmWriteServo(0, servo[4]);
 421             pwmWriteServo(1, servo[5]);
 422             break;
 423
 424         case MIXER_AIRPLANE:
 425         case MIXER_SINGLECOPTER:
 426             pwmWriteServo(0, servo[3]);
 427             pwmWriteServo(1, servo[4]);
 428             pwmWriteServo(2, servo[5]);
 429             pwmWriteServo(3, servo[6]);
 430             break;
 431
 432         default:
 433             // Two servos for SERVO_TILT, if enabled
 434             if (feature(FEATURE_SERVO_TILT)) {
 435                 updateGimbalServos();
 436             }
 437             break;
 438     }
 439 }
 440
 441 void writeMotors(void)
 442 {
 443     uint8_t i;
 444
 445     for (i = 0; i < motorCount; i++)
 446         pwmWriteMotor(i, motor[i]);
 447
 448
 449     if (feature(FEATURE_ONESHOT125)) {
 450         pwmCompleteOneshotMotorUpdate(motorCount);
 451     }
 452 }
 453
 454 void writeAllMotors(int16_t mc)
 455 {
 456     uint8_t i;
 457
 458     // Sends commands to all motors
 459     for (i = 0; i < motorCount; i++)
 460         motor[i] = mc;
 461     writeMotors();
 462 }
 463
 464 #ifndef USE_QUAD_MIXER_ONLY
 465 static void airplaneMixer(void)
 466 {
 467     int16_t flapperons[2] = { 0, 0 };
 468     int i;
 469
 470     if (!ARMING_FLAG(ARMED))
 471         servo[7] = escAndServoConfig->mincommand; // Kill throttle when disarmed
 472     else
 473         servo[7] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 474     motor[0] = servo[7];
 475
 476     if (airplaneConfig->flaps_speed) {
 477         // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
 478         // use servo min, servo max and servo rate for proper endpoints adjust
 479         static int16_t slow_LFlaps;
 480         int16_t lFlap = determineServoMiddleOrForwardFromChannel(2);
 481
 482         lFlap = constrain(lFlap, servoConf[2].min, servoConf[2].max);
 483         lFlap = escAndServoConfig->servoCenterPulse - lFlap;
 484         if (slow_LFlaps < lFlap)
 485             slow_LFlaps += airplaneConfig->flaps_speed;
 486         else if (slow_LFlaps > lFlap)
 487             slow_LFlaps -= airplaneConfig->flaps_speed;
 488
 489         servo[2] = ((int32_t)servoConf[2].rate * slow_LFlaps) / 100L;
 490         servo[2] += escAndServoConfig->servoCenterPulse;
 491     }
 492
 493     if (FLIGHT_MODE(PASSTHRU_MODE)) {   // Direct passthru from RX
 494         servo[3] = rcCommand[ROLL] + flapperons[0];     // Wing 1
 495         servo[4] = rcCommand[ROLL] + flapperons[1];     // Wing 2
 496         servo[5] = rcCommand[YAW];                      // Rudder
 497         servo[6] = rcCommand[PITCH];                    // Elevator
 498     } else {
 499         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
 500         servo[3] = axisPID[ROLL] + flapperons[0];       // Wing 1
 501         servo[4] = axisPID[ROLL] + flapperons[1];       // Wing 2
 502         servo[5] = axisPID[YAW];                        // Rudder
 503         servo[6] = axisPID[PITCH];                      // Elevator
 504     }
 505     for (i = 3; i < 7; i++) {
 506         servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; // servo rates
 507         servo[i] += determineServoMiddleOrForwardFromChannel(i);
 508     }
 509 }
 510 #endif
 511
 512 void mixTable(void)
 513 {
 514     int16_t maxMotor;
 515     uint32_t i;
 516
 517     if (motorCount > 3) {
 518         // prevent "yaw jump" during yaw correction
 519         axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW]));
 520     }
 521
 522     // motors for non-servo mixes
 523     if (motorCount > 1)
 524         for (i = 0; i < motorCount; i++)
 525             motor[i] =
 526                 rcCommand[THROTTLE] * currentMixer[i].throttle +
 527                 axisPID[PITCH] * currentMixer[i].pitch +
 528                 axisPID[ROLL] * currentMixer[i].roll +
 529                 -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
 530
 531 #ifndef USE_QUAD_MIXER_ONLY
 532     // airplane / servo mixes
 533     switch (currentMixerMode) {
 534         case MIXER_BI:
 535             servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4);     // LEFT
 536             servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5);     // RIGHT
 537             break;
 538
 539         case MIXER_TRI:
 540             servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
 541             break;
 542
 543         case MIXER_GIMBAL:
 544             servo[GIMBAL_SERVO_PITCH] = (((int32_t)servoConf[GIMBAL_SERVO_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_PITCH);
 545             servo[GIMBAL_SERVO_ROLL] = (((int32_t)servoConf[GIMBAL_SERVO_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_ROLL);
 546             break;
 547
 548         case MIXER_AIRPLANE:
 549             airplaneMixer();
 550             break;
 551
 552         case MIXER_FLYING_WING:
 553             if (!ARMING_FLAG(ARMED))
 554                 servo[7] = escAndServoConfig->mincommand;
 555             else
 556                 servo[7] = constrain(rcCommand[THROTTLE], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 557             motor[0] = servo[7];
 558             if (FLIGHT_MODE(PASSTHRU_MODE)) {
 559                 // do not use sensors for correction, simple 2 channel mixing
 560                 servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]);
 561                 servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]);
 562             } else {
 563                 // use sensors to correct (gyro only or gyro + acc)
 564                 servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]);
 565                 servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]);
 566             }
 567             servo[3] += determineServoMiddleOrForwardFromChannel(3);
 568             servo[4] += determineServoMiddleOrForwardFromChannel(4);
 569             break;
 570
 571         case MIXER_DUALCOPTER:
 572             for (i = 4; i < 6; i++) {
 573                 servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
 574                 servo[i] += determineServoMiddleOrForwardFromChannel(i);
 575             }
 576             break;
 577
 578         case MIXER_SINGLECOPTER:
 579             for (i = 3; i < 7; i++) {
 580                 servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
 581                 servo[i] += determineServoMiddleOrForwardFromChannel(i);
 582             }
 583             motor[0] = rcCommand[THROTTLE];
 584             break;
 585
 586         default:
 587             break;
 588     }
 589 #endif
 590
 591     // do camstab
 592     if (feature(FEATURE_SERVO_TILT)) {
 593         // center at fixed position, or vary either pitch or roll by RC channel
 594         servo[0] = determineServoMiddleOrForwardFromChannel(0);
 595         servo[1] = determineServoMiddleOrForwardFromChannel(1);
 596
 597         if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
 598             if (gimbalConfig->gimbal_flags & GIMBAL_MIXTILT) {
 599                 servo[0] -= (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 - (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50;
 600                 servo[1] += (-(int32_t)servoConf[0].rate) * inclination.values.pitchDeciDegrees / 50 + (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees / 50;
 601             } else {
 602                 servo[0] += (int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees / 50;
 603                 servo[1] += (int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees  / 50;
 604             }
 605         }
 606     }
 607
 608     // constrain servos
 609     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
 610         servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
 611
 612     // forward AUX1-4 to servo outputs (not constrained)
 613     if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
 614         // offset servos based off number already used in mixer types
 615         // airplane and servo_tilt together can't be used
 616         int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
 617
 618         // start forwarding from this channel
 619         uint8_t channelOffset = AUX1;
 620
 621         int8_t servoOffset;
 622         for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) {
 623             if (firstServo + servoOffset < 0) {
 624                 continue; // there are not enough servos to forward all the AUX channels.
 625             }
 626             pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
 627         }
 628     }
 629
 630     maxMotor = motor[0];
 631     for (i = 1; i < motorCount; i++)
 632         if (motor[i] > maxMotor)
 633             maxMotor = motor[i];
 634     for (i = 0; i < motorCount; i++) {
 635         if (maxMotor > escAndServoConfig->maxthrottle)     // this is a way to still have good gyro corrections if at least one motor reaches its max.
 636             motor[i] -= maxMotor - escAndServoConfig->maxthrottle;
 637         if (feature(FEATURE_3D)) {
 638             if ((rcData[THROTTLE]) > rxConfig->midrc) {
 639                 motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
 640             } else {
 641                 motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
 642             }
 643         } else {
 644             motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 645             if ((rcData[THROTTLE]) < rxConfig->mincheck) {
 646                 if (!feature(FEATURE_MOTOR_STOP))
 647                     motor[i] = escAndServoConfig->minthrottle;
 648                 else
 649                     motor[i] = escAndServoConfig->mincommand;
 650             }
 651         }
 652         if (!ARMING_FLAG(ARMED)) {
 653             motor[i] = motor_disarmed[i];
 654         }
 655     }
 656 }
 657
 658
 659 bool isMixerUsingServos(void)
 660 {
 661     return useServo;
 662 }