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 #include <string.h>
  22
  23 #include "platform.h"
  24 #include "build/debug.h"
  25
  26
  27 #include "build/build_config.h"
  28
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/filter.h"
  33
  34 #include "drivers/system.h"
  35 #include "drivers/pwm_output.h"
  36 #include "drivers/pwm_mapping.h"
  37 #include "drivers/sensor.h"
  38 #include "drivers/accgyro.h"
  39 #include "drivers/system.h"
  40
  41 #include "rx/rx.h"
  42
  43 #include "io/gimbal.h"
  44 #include "io/escservo.h"
  45 #include "fc/rc_controls.h"
  46
  47
  48 #include "sensors/sensors.h"
  49 #include "sensors/acceleration.h"
  50 #include "sensors/gyro.h"
  51
  52 #include "flight/mixer.h"
  53 #include "flight/failsafe.h"
  54 #include "flight/pid.h"
  55 #include "flight/imu.h"
  56 #include "flight/navigation_rewrite.h"
  57
  58 #include "fc/runtime_config.h"
  59
  60 #include "config/config.h"
  61 #include "config/config_profile.h"
  62
  63 //#define MIXER_DEBUG
  64
  65 uint8_t motorCount;
  66
  67 int16_t motor[MAX_SUPPORTED_MOTORS];
  68 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
  69
  70 bool motorLimitReached = false;
  71
  72 static mixerConfig_t *mixerConfig;
  73 static flight3DConfig_t *flight3DConfig;
  74 static escAndServoConfig_t *escAndServoConfig;
  75 static rxConfig_t *rxConfig;
  76
  77 static mixerMode_e currentMixerMode;
  78 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  79
  80
  81 #ifdef USE_SERVOS
  82 static uint8_t servoRuleCount = 0;
  83 static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
  84 static gimbalConfig_t *gimbalConfig;
  85 int16_t servo[MAX_SUPPORTED_SERVOS];
  86 static int servoOutputEnabled;
  87
  88 static uint8_t mixerUsesServos;
  89 static uint8_t minServoIndex;
  90 static uint8_t maxServoIndex;
  91
  92 static servoParam_t *servoConf;
  93 static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
  94 static bool servoFilterIsSet;
  95 #endif
  96
  97 static const motorMixer_t mixerQuadX[] = {
  98     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
  99     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 100     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 101     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 102 };
 103 #ifndef USE_QUAD_MIXER_ONLY
 104 static const motorMixer_t mixerTricopter[] = {
 105     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
 106     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
 107     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
 108 };
 109
 110 static const motorMixer_t mixerQuadP[] = {
 111     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
 112     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
 113     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
 114     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
 115 };
 116
 117 #ifndef DISABLE_UNCOMMON_MIXERS
 118 static const motorMixer_t mixerVtail4[] = {
 119     { 1.0f,  -0.58f,  0.58f, 1.0f },        // REAR_R
 120     { 1.0f,  -0.46f, -0.39f, -0.5f },       // FRONT_R
 121     { 1.0f,  0.58f,  0.58f, -1.0f },        // REAR_L
 122     { 1.0f,  0.46f, -0.39f, 0.5f },         // FRONT_L
 123 };
 124
 125 static const motorMixer_t mixerAtail4[] = {
 126     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_R
 127     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R
 128     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_L
 129     { 1.0f,  1.0f, -1.0f, -0.0f },          // FRONT_L
 130 };
 131
 132 static const motorMixer_t mixerY4[] = {
 133     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
 134     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
 135     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
 136     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
 137 };
 138
 139 #if (MAX_SUPPORTED_MOTORS >= 6)
 140 static const motorMixer_t mixerHex6H[] = {
 141     { 1.0f, -1.0f,  1.0f, -1.0f },     // REAR_R
 142     { 1.0f, -1.0f, -1.0f,  1.0f },     // FRONT_R
 143     { 1.0f,  1.0f,  1.0f,  1.0f },     // REAR_L
 144     { 1.0f,  1.0f, -1.0f, -1.0f },     // FRONT_L
 145     { 1.0f,  0.0f,  0.0f,  0.0f },     // RIGHT
 146     { 1.0f,  0.0f,  0.0f,  0.0f },     // LEFT
 147 };
 148
 149 static const motorMixer_t mixerY6[] = {
 150     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 151     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 152     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 153     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 154     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 155     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 156 };
 157
 158 static const motorMixer_t mixerHex6P[] = {
 159     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 160     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 161     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 162     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 163     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 164     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 165 };
 166 #endif
 167
 168 #if (MAX_SUPPORTED_MOTORS >= 8)
 169 static const motorMixer_t mixerOctoFlatP[] = {
 170     { 1.0f,  0.707107f, -0.707107f,  1.0f },    // FRONT_L
 171     { 1.0f, -0.707107f, -0.707107f,  1.0f },    // FRONT_R
 172     { 1.0f, -0.707107f,  0.707107f,  1.0f },    // REAR_R
 173     { 1.0f,  0.707107f,  0.707107f,  1.0f },    // REAR_L
 174     { 1.0f,  0.0f, -1.0f, -1.0f },              // FRONT
 175     { 1.0f, -1.0f,  0.0f, -1.0f },              // RIGHT
 176     { 1.0f,  0.0f,  1.0f, -1.0f },              // REAR
 177     { 1.0f,  1.0f,  0.0f, -1.0f },              // LEFT
 178 };
 179
 180 static const motorMixer_t mixerOctoFlatX[] = {
 181     { 1.0f,  1.0f, -0.414178f,  1.0f },      // MIDFRONT_L
 182     { 1.0f, -0.414178f, -1.0f,  1.0f },      // FRONT_R
 183     { 1.0f, -1.0f,  0.414178f,  1.0f },      // MIDREAR_R
 184     { 1.0f,  0.414178f,  1.0f,  1.0f },      // REAR_L
 185     { 1.0f,  0.414178f, -1.0f, -1.0f },      // FRONT_L
 186     { 1.0f, -1.0f, -0.414178f, -1.0f },      // MIDFRONT_R
 187     { 1.0f, -0.414178f,  1.0f, -1.0f },      // REAR_R
 188     { 1.0f,  1.0f,  0.414178f, -1.0f },      // MIDREAR_L
 189 };
 190
 191 static const motorMixer_t mixerOctoX8[] = {
 192     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 193     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 194     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 195     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 196     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 197     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 198     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 199     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 200 };
 201 #endif
 202 #endif //DISABLE_UNCOMMON_MIXERS
 203
 204 #if (MAX_SUPPORTED_MOTORS >= 6)
 205 static const motorMixer_t mixerHex6X[] = {
 206     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 207     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 208     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 209     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 210     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 211     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 212 };
 213 #endif
 214
 215 static const motorMixer_t mixerSingleProp[] = {
 216     { 1.0f,  0.0f,  0.0f, 0.0f },
 217 };
 218
 219 // Keep synced with mixerMode_e
 220 const mixer_t mixers[] = {
 221     // motors, use servo, motor mixer
 222     { 0, false, NULL, true },                // entry 0
 223     { 3, true,  mixerTricopter, true },      // MIXER_TRI
 224     { 4, false, mixerQuadP, true },          // MIXER_QUADP
 225     { 4, false, mixerQuadX, true },          // MIXER_QUADX
 226
 227     { 0, false, NULL, false },               // MIXER_BICOPTER
 228     { 0, false, NULL, false },               // MIXER_GIMBAL -> this mixer was never implemented in CF, use feature(FEATURE_SERVO_TILT) instead
 229     #if !defined(DISABLE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 6)
 230         { 6, false, mixerY6, true },         // MIXER_Y6
 231         { 6, false, mixerHex6P, true },      // MIXER_HEX6
 232     #else
 233         { 0, false, NULL, false },           // MIXER_Y6
 234         { 0, false, NULL, false },           // MIXER_HEX6
 235     #endif
 236     { 1, true,  mixerSingleProp, true },     // MIXER_FLYING_WING
 237     #if !defined(DISABLE_UNCOMMON_MIXERS)
 238         { 4, false, mixerY4, true },         // MIXER_Y4
 239     #else
 240         { 0, false, NULL, false },           // MIXER_Y4
 241     #endif
 242     #if (MAX_SUPPORTED_MOTORS >= 6)
 243         { 6, false, mixerHex6X, true },          // MIXER_HEX6X
 244     #else
 245         { 0, false, NULL, false },          // MIXER_HEX6X
 246     #endif
 247     #if !defined(DISABLE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8)
 248         { 8, false, mixerOctoX8, true },     // MIXER_OCTOX8
 249         { 8, false, mixerOctoFlatP, true },  // MIXER_OCTOFLATP
 250         { 8, false, mixerOctoFlatX, true },  // MIXER_OCTOFLATX
 251     #else
 252         { 0, false, NULL, false },           // MIXER_OCTOX8
 253         { 0, false, NULL, false },           // MIXER_OCTOFLATP
 254         { 0, false, NULL, false },           // MIXER_OCTOFLATX
 255     #endif
 256     { 1, true,  mixerSingleProp, true },     // * MIXER_AIRPLANE
 257     { 0, true,  NULL, false },               // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF
 258     { 0, true,  NULL, false },               // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF
 259     #if !defined(DISABLE_UNCOMMON_MIXERS)
 260         { 4, false, mixerVtail4, true },     // MIXER_VTAIL4
 261     #if (MAX_SUPPORTED_MOTORS >= 6)
 262         { 6, false, mixerHex6H, true },      // MIXER_HEX6H
 263     #else
 264         { 0, false, NULL, false },           // MIXER_HEX6H
 265     #endif
 266     #else
 267         { 0, false, NULL, false },           // MIXER_VTAIL4
 268         { 0, false, NULL, false },           // MIXER_HEX6H
 269     #endif
 270     { 0, true,  NULL, false },               // * MIXER_PPM_TO_SERVO -> looks like this is not implemented at all
 271     { 0, false, NULL, false },               // MIXER_DUALCOPTER
 272     { 0, false, NULL, false },               // MIXER_SINGLECOPTER
 273     #if !defined(DISABLE_UNCOMMON_MIXERS)
 274         { 4, false, mixerAtail4, true },     // MIXER_ATAIL4
 275     #else
 276         { 0, false, NULL, false },           // MIXER_ATAIL4
 277     #endif
 278     { 0, false, NULL, true },                // MIXER_CUSTOM
 279     { 2, true,  NULL, true },                // MIXER_CUSTOM_AIRPLANE
 280     { 3, true,  NULL, true },                // MIXER_CUSTOM_TRI
 281 };
 282 #endif // USE_QUAD_MIXER_ONLY
 283
 284 #ifdef USE_SERVOS
 285
 286 #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
 287 // mixer rule format servo, input, rate, speed, min, max, box
 288 static const servoMixer_t servoMixerAirplane[] = {
 289     { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 290     { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 291     { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 292     { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 293     { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
 294 };
 295
 296 static const servoMixer_t servoMixerFlyingWing[] = {
 297     { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
 298     { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 299     { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  -100, 0, 0, 100, 0 },
 300     { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
 301     { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
 302 };
 303
 304 static const servoMixer_t servoMixerTri[] = {
 305     { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
 306 };
 307
 308 const mixerRules_t servoMixers[] = {
 309     { 0, 0, 0, NULL },                // entry 0
 310     { COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri },       // MULTITYPE_TRI
 311     { 0, 0, 0, NULL },                // MULTITYPE_QUADP
 312     { 0, 0, 0, NULL },                // MULTITYPE_QUADX
 313     { 0, 0, 0, NULL },                // MULTITYPE_BI
 314     { 0, 0, 0, NULL },                // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled
 315     { 0, 0, 0, NULL },                // MULTITYPE_Y6
 316     { 0, 0, 0, NULL },                // MULTITYPE_HEX6
 317     { COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
 318     { 0, 0, 0, NULL },                // MULTITYPE_Y4
 319     { 0, 0, 0, NULL },                // MULTITYPE_HEX6X
 320     { 0, 0, 0, NULL },                // MULTITYPE_OCTOX8
 321     { 0, 0, 0, NULL },                // MULTITYPE_OCTOFLATP
 322     { 0, 0, 0, NULL },                // MULTITYPE_OCTOFLATX
 323     { COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane },  // * MULTITYPE_AIRPLANE
 324     { 0, 0, 0, NULL },                // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF
 325     { 0, 0, 0, NULL },                // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF
 326     { 0, 0, 0, NULL },                // MULTITYPE_VTAIL4
 327     { 0, 0, 0, NULL },                // MULTITYPE_HEX6H
 328     { 0, 0, 0, NULL },                // * MULTITYPE_PPM_TO_SERVO
 329     { 0, 0, 0, NULL },                // MULTITYPE_DUALCOPTER
 330     { 0, 0, 0, NULL },                // MULTITYPE_SINGLECOPTER
 331     { 0, 0, 0, NULL },                // MULTITYPE_ATAIL4
 332     { 0, 2, 5, NULL },                // MULTITYPE_CUSTOM
 333     { 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL },                // MULTITYPE_CUSTOM_PLANE
 334     { 0, SERVO_RUDDER, SERVO_RUDDER, NULL },                // MULTITYPE_CUSTOM_TRI
 335 };
 336
 337 static servoMixer_t *customServoMixers;
 338 #endif
 339
 340
 341 static motorMixer_t *customMixers;
 342
 343 void mixerUseConfigs(
 344 #ifdef USE_SERVOS
 345         servoParam_t *servoConfToUse,
 346         gimbalConfig_t *gimbalConfigToUse,
 347 #endif
 348         flight3DConfig_t *flight3DConfigToUse,
 349         escAndServoConfig_t *escAndServoConfigToUse,
 350         mixerConfig_t *mixerConfigToUse,
 351         rxConfig_t *rxConfigToUse)
 352 {
 353 #ifdef USE_SERVOS
 354     servoConf = servoConfToUse;
 355     gimbalConfig = gimbalConfigToUse;
 356 #endif
 357     flight3DConfig = flight3DConfigToUse;
 358     escAndServoConfig = escAndServoConfigToUse;
 359     mixerConfig = mixerConfigToUse;
 360     rxConfig = rxConfigToUse;
 361 }
 362
 363 #ifdef USE_SERVOS
 364
 365 int16_t getFlaperonDirection(uint8_t servoPin) {
 366     if (servoPin == SERVO_FLAPPERON_2) {
 367         return -1;
 368     } else {
 369         return 1;
 370     }
 371 }
 372
 373 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
 374 {
 375     uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
 376
 377     if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
 378         return rcData[channelToForwardFrom];
 379     }
 380
 381     return servoConf[servoIndex].middle;
 382 }
 383
 384
 385 int servoDirection(int servoIndex, int inputSource)
 386 {
 387     // determine the direction (reversed or not) from the direction bitfield of the servo
 388     if (servoConf[servoIndex].reversedSources & (1 << inputSource))
 389         return -1;
 390     else
 391         return 1;
 392 }
 393 #endif
 394
 395 bool isMixerEnabled(mixerMode_e mixerMode)
 396 {
 397 #ifdef USE_QUAD_MIXER_ONLY
 398     UNUSED(mixerMode);
 399     return true;
 400 #else
 401     return mixers[mixerMode].enabled;
 402 #endif
 403 }
 404
 405 #ifdef USE_SERVOS
 406 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers)
 407 {
 408     int i;
 409
 410     currentMixerMode = mixerMode;
 411
 412     // set flag that we're on something with wings
 413     if (currentMixerMode == MIXER_FLYING_WING ||
 414         currentMixerMode == MIXER_AIRPLANE ||
 415         currentMixerMode == MIXER_CUSTOM_AIRPLANE
 416     ) {
 417         ENABLE_STATE(FIXED_WING);
 418     } else {
 419         DISABLE_STATE(FIXED_WING);
 420     }
 421
 422     if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
 423         ENABLE_STATE(FLAPERON_AVAILABLE);
 424     } else {
 425         DISABLE_STATE(FLAPERON_AVAILABLE);
 426     }
 427
 428     customMixers = initialCustomMotorMixers;
 429     customServoMixers = initialCustomServoMixers;
 430
 431     minServoIndex = servoMixers[currentMixerMode].minServoIndex;
 432     maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
 433
 434     // enable servos for mixes that require them. note, this shifts motor counts.
 435     mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT);
 436
 437     // if we want camstab/trig, that also enables servos, even if mixer doesn't
 438     servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING);
 439
 440     // give all servos a default command
 441     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 442         servo[i] = DEFAULT_SERVO_MIDDLE;
 443     }
 444
 445     /*
 446      * If mixer has predefined servo mixes, load them
 447      */
 448     if (mixerUsesServos) {
 449         servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
 450         if (servoMixers[currentMixerMode].rule) {
 451             for (i = 0; i < servoRuleCount; i++)
 452                 currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
 453         }
 454     }
 455
 456     /*
 457      * When we are dealing with CUSTOM mixers, load mixes defined with
 458      * smix and update internal variables
 459      */
 460     if (currentMixerMode == MIXER_CUSTOM_AIRPLANE ||
 461         currentMixerMode == MIXER_CUSTOM_TRI ||
 462         currentMixerMode == MIXER_CUSTOM)
 463     {
 464         loadCustomServoMixer();
 465
 466         // If there are servo rules after all, update variables
 467         if (servoRuleCount > 0) {
 468             servoOutputEnabled = 1;
 469             mixerUsesServos = 1;
 470         }
 471
 472     }
 473 }
 474 #else
 475 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
 476 {
 477     currentMixerMode = mixerMode;
 478     customMixers = initialCustomMixers;
 479 }
 480 #endif
 481
 482 #ifdef USE_SERVOS
 483 void mixerUsePWMIOConfiguration(void)
 484 {
 485     int i;
 486
 487     motorCount = 0;
 488
 489     if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
 490         // load custom mixer into currentMixer
 491         for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 492             // check if done
 493             if (customMixers[i].throttle == 0.0f)
 494                 break;
 495             currentMixer[i] = customMixers[i];
 496             motorCount++;
 497         }
 498     } else {
 499         motorCount = mixers[currentMixerMode].motorCount;
 500         // copy motor-based mixers
 501         if (mixers[currentMixerMode].motor) {
 502             for (i = 0; i < motorCount; i++)
 503                 currentMixer[i] = mixers[currentMixerMode].motor[i];
 504         }
 505     }
 506
 507     // in 3D mode, mixer gain has to be halved
 508     if (feature(FEATURE_3D)) {
 509         if (motorCount > 1) {
 510             for (i = 0; i < motorCount; i++) {
 511                 currentMixer[i].pitch *= 0.5f;
 512                 currentMixer[i].roll *= 0.5f;
 513                 currentMixer[i].yaw *= 0.5f;
 514             }
 515         }
 516     }
 517
 518     mixerResetDisarmedMotors();
 519 }
 520 #else
 521 void mixerUsePWMIOConfiguration(void)
 522 {
 523     motorCount = 4;
 524     int i;
 525     for (i = 0; i < motorCount; i++) {
 526         currentMixer[i] = mixerQuadX[i];
 527     }
 528     mixerResetDisarmedMotors();
 529 }
 530 #endif
 531
 532
 533 #ifndef USE_QUAD_MIXER_ONLY
 534 #ifdef USE_SERVOS
 535 void loadCustomServoMixer(void)
 536 {
 537     int i;
 538
 539     // reset settings
 540     servoRuleCount = 0;
 541     minServoIndex = 255;
 542     maxServoIndex = 0;
 543     memset(currentServoMixer, 0, sizeof(currentServoMixer));
 544
 545     // load custom mixer into currentServoMixer
 546     for (i = 0; i < MAX_SERVO_RULES; i++) {
 547         // check if done
 548         if (customServoMixers[i].rate == 0)
 549             break;
 550
 551         if (customServoMixers[i].targetChannel < minServoIndex) {
 552             minServoIndex = customServoMixers[i].targetChannel;
 553         }
 554
 555         if (customServoMixers[i].targetChannel > maxServoIndex) {
 556             maxServoIndex = customServoMixers[i].targetChannel;
 557         }
 558
 559         currentServoMixer[i] = customServoMixers[i];
 560         servoRuleCount++;
 561     }
 562 }
 563
 564 void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
 565 {
 566     int i;
 567
 568     // we're 1-based
 569     index++;
 570     // clear existing
 571     for (i = 0; i < MAX_SERVO_RULES; i++)
 572         customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
 573
 574     for (i = 0; i < servoMixers[index].servoRuleCount; i++) {
 575         customServoMixers[i] = servoMixers[index].rule[i];
 576     }
 577 }
 578 #endif
 579
 580
 581 void mixerLoadMix(int index, motorMixer_t *customMixers)
 582 {
 583     int i;
 584
 585     // we're 1-based
 586     index++;
 587     // clear existing
 588     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 589         customMixers[i].throttle = 0.0f;
 590
 591     // do we have anything here to begin with?
 592     if (mixers[index].motor != NULL) {
 593         for (i = 0; i < mixers[index].motorCount; i++)
 594             customMixers[i] = mixers[index].motor[i];
 595     }
 596 }
 597
 598 #endif
 599
 600 void mixerResetDisarmedMotors(void)
 601 {
 602     int i;
 603     // set disarmed motor values
 604     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 605         motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
 606 }
 607
 608 #ifdef USE_SERVOS
 609
 610 STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
 611 {
 612     // start forwarding from this channel
 613     uint8_t channelOffset = AUX1;
 614
 615     int servoOffset;
 616     for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
 617         pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
 618     }
 619 }
 620
 621 void writeServos(void)
 622 {
 623     int servoIndex = 0;
 624
 625     bool zeroServoValue = false;
 626
 627     /*
 628      * in case of tricopters, there might me a need to zero servo output when unarmed
 629      */
 630     if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) {
 631         zeroServoValue = true;
 632     }
 633
 634     // Write mixer servo outputs
 635     //      mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos
 636     if (mixerUsesServos && servoRuleCount) {
 637         for (int i = minServoIndex; i <= maxServoIndex; i++) {
 638             if (zeroServoValue) {
 639                 pwmWriteServo(servoIndex++, 0);
 640             } else {
 641                 pwmWriteServo(servoIndex++, servo[i]);
 642             }
 643         }
 644     }
 645
 646     if (feature(FEATURE_SERVO_TILT)) {
 647         pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]);
 648         pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]);
 649     }
 650
 651     // forward AUX to remaining servo outputs (not constrained)
 652     if (feature(FEATURE_CHANNEL_FORWARDING)) {
 653         forwardAuxChannelsToServos(servoIndex);
 654         servoIndex += MAX_AUX_CHANNEL_COUNT;
 655     }
 656 }
 657 #endif
 658
 659 void writeMotors(void)
 660 {
 661     int i;
 662
 663     for (i = 0; i < motorCount; i++)
 664         pwmWriteMotor(i, motor[i]);
 665
 666
 667     if (feature(FEATURE_ONESHOT125)) {
 668         pwmCompleteOneshotMotorUpdate(motorCount);
 669     }
 670 }
 671
 672 void writeAllMotors(int16_t mc)
 673 {
 674     int i;
 675
 676     // Sends commands to all motors
 677     for (i = 0; i < motorCount; i++)
 678         motor[i] = mc;
 679     writeMotors();
 680 }
 681
 682 void stopMotors(void)
 683 {
 684     writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand);
 685
 686     delay(50); // give the timers and ESCs a chance to react.
 687 }
 688
 689 void StopPwmAllMotors()
 690 {
 691     pwmShutdownPulsesForAllMotors(motorCount);
 692 }
 693
 694 void mixTable(void)
 695 {
 696     int i;
 697
 698     if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
 699         // prevent "yaw jump" during yaw correction
 700         axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
 701     }
 702
 703     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
 704     int16_t rpyMix[MAX_SUPPORTED_MOTORS];
 705     int16_t rpyMixMax = 0; // assumption: symetrical about zero.
 706     int16_t rpyMixMin = 0;
 707
 708     // motors for non-servo mixes
 709     for (i = 0; i < motorCount; i++) {
 710         rpyMix[i] =
 711             axisPID[PITCH] * currentMixer[i].pitch +
 712             axisPID[ROLL] * currentMixer[i].roll +
 713             -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
 714
 715         if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
 716         if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
 717     }
 718
 719     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
 720     int16_t throttleRange, throttleCommand;
 721     int16_t throttleMin, throttleMax;
 722     static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 723
 724     // Find min and max throttle based on condition.
 725     if (feature(FEATURE_3D)) {
 726         if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 727
 728         if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
 729             throttleMax = flight3DConfig->deadband3d_low;
 730             throttleMin = escAndServoConfig->minthrottle;
 731             throttlePrevious = throttleCommand = rcCommand[THROTTLE];
 732         } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
 733             throttleMax = escAndServoConfig->maxthrottle;
 734             throttleMin = flight3DConfig->deadband3d_high;
 735             throttlePrevious = throttleCommand = rcCommand[THROTTLE];
 736         } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)))  { // Deadband handling from negative to positive
 737             throttleCommand = throttleMax = flight3DConfig->deadband3d_low;
 738             throttleMin = escAndServoConfig->minthrottle;
 739         } else {  // Deadband handling from positive to negative
 740             throttleMax = escAndServoConfig->maxthrottle;
 741             throttleCommand = throttleMin = flight3DConfig->deadband3d_high;
 742         }
 743     } else {
 744         throttleCommand = rcCommand[THROTTLE];
 745         throttleMin = escAndServoConfig->minthrottle;
 746         throttleMax = escAndServoConfig->maxthrottle;
 747     }
 748
 749     throttleRange = throttleMax - throttleMin;
 750
 751     #define THROTTLE_CLIPPING_FACTOR    0.33f
 752     if (rpyMixRange > throttleRange) {
 753         motorLimitReached = true;
 754         float mixReduction = (float)throttleRange / rpyMixRange;
 755
 756         for (i = 0; i < motorCount; i++) {
 757             rpyMix[i] =  mixReduction  * rpyMix[i];
 758         }
 759
 760         // Allow some clipping on edges to soften correction response
 761         throttleMin = throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 762         throttleMax = throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 763     } else {
 764         motorLimitReached = false;
 765         throttleMin = MIN(throttleMin + (rpyMixRange / 2), throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 766         throttleMax = MAX(throttleMax - (rpyMixRange / 2), throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 767     }
 768
 769     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 770     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 771     if (ARMING_FLAG(ARMED)) {
 772         bool isFailsafeActive = failsafeIsActive();
 773
 774         for (i = 0; i < motorCount; i++) {
 775             motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
 776
 777             if (isFailsafeActive) {
 778                 motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
 779             } else if (feature(FEATURE_3D)) {
 780                 if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
 781                     motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
 782                 } else {
 783                     motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
 784                 }
 785             } else {
 786                 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 787             }
 788
 789             // Motor stop handling
 790             if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D)) {
 791                 if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
 792                     motor[i] = escAndServoConfig->mincommand;
 793                 }
 794             }
 795         }
 796     } else {
 797         for (i = 0; i < motorCount; i++) {
 798             motor[i] = motor_disarmed[i];
 799         }
 800     }
 801 }
 802
 803 #ifdef USE_SERVOS
 804
 805 void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)
 806 {
 807     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
 808     static int16_t currentOutput[MAX_SERVO_RULES];
 809     int i;
 810
 811     if (FLIGHT_MODE(PASSTHRU_MODE)) {
 812         // Direct passthru from RX
 813         input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
 814         input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
 815         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
 816     } else {
 817         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
 818         input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
 819         input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
 820         input[INPUT_STABILIZED_YAW] = axisPID[YAW];
 821
 822         // Reverse yaw servo when inverted in 3D mode
 823         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
 824             input[INPUT_STABILIZED_YAW] *= -1;
 825         }
 826     }
 827
 828     input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
 829     input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
 830
 831     input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]
 832
 833     // center the RC input value around the RC middle value
 834     // by subtracting the RC middle value from the RC input value, we get:
 835     // data - middle = input
 836     // 2000 - 1500 = +500
 837     // 1500 - 1500 = 0
 838     // 1000 - 1500 = -500
 839     input[INPUT_RC_ROLL]     = rcData[ROLL]     - rxConfig->midrc;
 840     input[INPUT_RC_PITCH]    = rcData[PITCH]    - rxConfig->midrc;
 841     input[INPUT_RC_YAW]      = rcData[YAW]      - rxConfig->midrc;
 842     input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
 843     input[INPUT_RC_AUX1]     = rcData[AUX1]     - rxConfig->midrc;
 844     input[INPUT_RC_AUX2]     = rcData[AUX2]     - rxConfig->midrc;
 845     input[INPUT_RC_AUX3]     = rcData[AUX3]     - rxConfig->midrc;
 846     input[INPUT_RC_AUX4]     = rcData[AUX4]     - rxConfig->midrc;
 847
 848     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
 849         servo[i] = 0;
 850
 851     // mix servos according to rules
 852     for (i = 0; i < servoRuleCount; i++) {
 853         // consider rule if no box assigned or box is active
 854         if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
 855             uint8_t target = currentServoMixer[i].targetChannel;
 856             uint8_t from = currentServoMixer[i].inputSource;
 857             uint16_t servo_width = servoConf[target].max - servoConf[target].min;
 858             int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
 859             int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
 860
 861             if (currentServoMixer[i].speed == 0) {
 862                 currentOutput[i] = input[from];
 863             } else {
 864                 if (currentOutput[i] < input[from]) {
 865                     currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
 866                 } else if (currentOutput[i] > input[from]) {
 867                     currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
 868                 }
 869             }
 870
 871             /*
 872             Flaperon fligh mode
 873             */
 874             if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
 875                 int8_t multiplier = 1;
 876
 877                 if (flaperon_throw_inverted == 1) {
 878                     multiplier = -1;
 879                 }
 880                 currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
 881             }
 882
 883             servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
 884         } else {
 885             currentOutput[i] = 0;
 886         }
 887     }
 888
 889     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 890         servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
 891         servo[i] += determineServoMiddleOrForwardFromChannel(i);
 892     }
 893 }
 894
 895 void processServoTilt(void) {
 896     // center at fixed position, or vary either pitch or roll by RC channel
 897     servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
 898     servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
 899
 900     if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
 901         if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
 902             servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
 903             servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
 904         } else {
 905             servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
 906             servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll  / 50;
 907         }
 908     }
 909 }
 910
 911 bool isServoOutputEnabled(void)
 912 {
 913     return servoOutputEnabled;
 914 }
 915
 916 bool isMixerUsingServos(void) {
 917     return mixerUsesServos;
 918 }
 919
 920 void filterServos(void)
 921 {
 922     int servoIdx;
 923
 924     if (mixerConfig->servo_lowpass_enable) {
 925         // Initialize servo lowpass filter (servos are calculated at looptime rate)
 926         if (!servoFilterIsSet) {
 927             for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
 928                 biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
 929             }
 930
 931             servoFilterIsSet = true;
 932         }
 933
 934         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
 935             // Apply servo lowpass filter and do sanity cheching
 936             servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
 937         }
 938     }
 939
 940     for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
 941         servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
 942     }
 943 }
 944 #endif